#include "sci_lio/lio.h"
#include "sci_lio/timer.h"
#include <mutex>

#define YEL
std::mutex Max_Group_mutex;
std::mutex Current_Voxel_mutex;
extern Timer timer;
bool add_overlap = false;
//是否使用sc2得分
bool sc2 = true;
//INT_MAX最大值2^31-1
int max_est_num = INT_MAX;
//不使用描述子
std::string descriptor = "NULL";
std::chrono::duration<double> elapsed_time;
int per_gourp_num = 100;

//使用了递归的 timer.Evaluate 方法来确保在执行耗时操作（如去畸变和降采样）时不会阻塞主线程
bool LIO::MeasurementUpdate(SensorMeasurement& sensor_measurement) 
{
  //! 对激光雷达 sensor_measurement 进行过滤和降采样操作
  if (sensor_measurement.measurement_type_ == MeasurementType::LIDAR) 
  {
    filtered_cloud_ptr->points.clear();
    filtered_cloud_ptr->points.reserve(sensor_measurement.cloud_ptr_->size());

    //! 将符合条件的点（InRadius）添加到过滤后的云 filtered_cloud_ptr 中
    for (const auto& pt : sensor_measurement.cloud_ptr_->points) 
    {
      if (InRadius(pt))   
        filtered_cloud_ptr->points.emplace_back(pt);
    }
    sensor_measurement.cloud_ptr_ = filtered_cloud_ptr;

    //! 将激光雷达坐标系的点云转换到 IMU 坐标系中。将转换后的云赋值给 sensor_measurement.cloud_ptr_。
    timer.Evaluate([&, this]() 
    {
          CloudPtr cloud_body_ptr(new CloudType());
          pcl::transformPointCloud(*sensor_measurement.cloud_ptr_, *cloud_body_ptr, config_.T_imu_lidar);
          sensor_measurement.cloud_ptr_ = std::move(cloud_body_ptr);

          //! UndistortPointCloud方法对sensor_measurement.cloud_ptr_进行去畸变操作
          if(config_.enable_undistort)  
            UndistortPointCloud(sensor_measurement.bag_time_, sensor_measurement.lidar_end_time_, sensor_measurement.cloud_ptr_);
    }, "undistort");
    filtered_cloud_ptr = sensor_measurement.cloud_ptr_;

    //! 调用 fast_voxel_grid_ptr_ 的 Filter 方法对 sensor_measurement.cloud_ptr_ 进行降采样操作。将降采样后的云存储在 cloud_DS_ptr_ 和 cloud_cov_ptr_ 中
    //! YEL：将当前帧的每个点，根据预测的T，转换到世界坐标系中，进行voxel体素化，计算体素的质心和协方差矩阵。
    timer.Evaluate([&, this]() 
    {
      fast_voxel_grid_ptr_->Filter(sensor_measurement.cloud_ptr_, cloud_DS_ptr_, cloud_cov_ptr_, curr_state_.pose.block<3,3>(0,0), curr_state_.pose.block<3,1>(0,3));
    },"downsample");
  }

  //! 确保局部地图足够稠密（lidar_frame_count_大于10），才能进行量测更新
  if (lidar_frame_count_ <= 10) 
  {
    CloudPtr trans_cloud_ptr(new CloudType());
    //! 将点云转换到世界坐标系下，并加入到Voxel地图voxel_map_ptr_
    pcl::transformPointCloud(*sensor_measurement.cloud_ptr_, *trans_cloud_ptr, curr_state_.pose);
    voxel_map_ptr_->AddCloud(trans_cloud_ptr);
    lidar_frame_count_++;
    return true;
  }
  LOG(INFO) << "-------MeasurementUpdate Start-------" << std::endl;
  
  //! 量测更新:将当前状态（curr_state_）存储在prev_state_中，并将迭代次数（iter_num_）设置为0。同时，设置一个标志位need_converge_，用于判断是否需要收敛。
  prev_state_ = curr_state_;
  iter_num_ = 0;
  need_converge_ = false;
  Eigen::Matrix<double, 15, 1> delta_x = Eigen::Matrix<double, 15, 1>::Zero();
  while (iter_num_ < config_.max_iterations) 
  {
    //! 非常关键的函数
    StepOptimize(sensor_measurement, delta_x);

    //! 根据delta_x判定是否收敛，如果收敛就中断优化；否则继续迭代优化（注意前三个迭代需要kNN搜索最近点来估计平面信息，从第四次开始就不用了）
    if (IsConverged(delta_x)) 
    {
      LOG(INFO)<<"POS:"<<curr_state_.pose.block<3, 1>(0, 3);
      break;
    } 
    else 
    {
      //前4次（iter_num_等于0、1、2、3）执行KNN搜索最近点来估计平面信息，以后不再执行，来加速收敛
      if (iter_num_ < 3) 
        {need_converge_ = false;} 
      else 
        {need_converge_ = true;}
    }
    iter_num_++;
  }

  //! 更新最终的协方差
  ComputeFinalCovariance(delta_x);
  //! 更新上一帧状态
  prev_state_ = curr_state_;
  //! 更新和管理点云数据
  timer.Evaluate([&, this]() 
  {
    //! 当激光雷达帧计数（lidar_frame_count_）小于10时，代码将创建一个新的点云对象，并将其转换为当前状态（curr_state_pose）下的坐标。然后，它将这个点云数据添加到体素地图中（voxel_map_ptr_）
    if (lidar_frame_count_ < 10) 
    {
      CloudPtr trans_cloud_ptr(new CloudType());
      pcl::transformPointCloud(*sensor_measurement.cloud_ptr_,*trans_cloud_ptr,curr_state_.pose);
      voxel_map_ptr_->AddCloud(trans_cloud_ptr);

      last_keyframe_pose_ = curr_state_.pose;
    } 
    else 
    {
      //! 更新阶段：当激光雷达帧计数大于等于10时，代码将计算当前状态下的坐标（curr_state_pose）与上一个关键帧状态下的坐标（last_keyframe_pose_）之间的差值（delta_p）
      Eigen::Matrix4d delta_p = last_keyframe_pose_.inverse() * curr_state_.pose;
      //! 如果满足以下条件之一，则认为需要添加一个新的关键帧
      //TODO::这里的条件可以根据实际情况进行调整
      if (effect_feat_num_ < 1000 ||                    //有效特征数量（effect_feat_num_）小于1000，这通常意味着点云数据不够密集，需要更多的关键帧来提高特征识别能力
          delta_p.block<3, 1>(0, 3).norm() > 0.7 ||     //坐标变化量（delta_p）大于1，这表明当前状态与上一个关键帧有较大的位移，需要一个新的关键帧来确保位移得到正确处理。TODO::原来的是0.5
          Sophus::SO3d(delta_p.block<3, 3>(0, 0)).log().norm() > 0.18) 
        {   
          CloudPtr trans_cloud_DS_ptr(new CloudType());
          pcl::transformPointCloud(*cloud_DS_ptr_, *trans_cloud_DS_ptr, curr_state_.pose);
          voxel_map_ptr_->AddCloud(trans_cloud_DS_ptr);

          last_keyframe_pose_ = curr_state_.pose;
          keyframe_count_++;
        }
    }
  },"update voxel map");

  lidar_frame_count_++;

  ava_effect_feat_num_ += (effect_feat_num_ - ava_effect_feat_num_) / static_cast<double>(lidar_frame_count_);

  return true;
}

//计算 sensor_measurement 类型的传感器测量产生的约束条件，然后使用Gauss-Newton步进优化方法更新delta_x
/*
①关键帧数量keyframe_count_大于20时，ConstructGICPConstraints基于GICP的约束条件：
②关键帧数量keyframe_count_小于20时，ConstructPoint2PlaneConstraints基于点-面方法的约束条件：
③ConstructImuPriorConstraints构造基于IMU的约束条件：
④基于约束条件，执行Gauss-Newton优化
*/
//! gauss_newton_step用于执行Gauss-Newton步进优化，它是实现优化算法的核心
bool LIO::StepOptimize(const SensorMeasurement& sensor_measurement,Eigen::Matrix<double, 15, 1>& delta_x) 
{
  Eigen::Matrix<double, 15, 15> H = Eigen::Matrix<double, 15, 15>::Zero();
  Eigen::Matrix<double, 15, 1> b = Eigen::Matrix<double, 15, 1>::Zero();

  double y0 = 0;
  switch (sensor_measurement.measurement_type_) 
  {
    case MeasurementType::LIDAR: 
    {
      double y0_lidar = 0.0;

      //! ConstructGICPConstraints和ConstructPoint2PlaneConstraints分别用于构造基于GICP和点-面方法的约束条件
      timer.Evaluate([&, this]() 
      {
        if (keyframe_count_ > 20) 
        {
          //! 创新点:使用Wasserstein距离的GICP约束
          y0_lidar = ConstructWassersteinGICPConstraints(H, b);
          //y0_lidar = ConstructGICPConstraints(H, b);
        }
        //! 初始阶段：由于the probability of each voxel is poor，使用点-面方法的约束条件
        else 
        {
          y0_lidar = ConstructPoint2PlaneConstraints(H, b);
        }
      },"lidar constraints");
      y0 += y0_lidar;
      break;
    }
    default: 
    {
      LOG(ERROR) << "error measurement type!";
      exit(0);
    }
  }

  //! 函数ConstructImuPriorConstraints用于构造基于IMU的约束条件
  timer.Evaluate([&, this]() 
  {
    double y0_imu = ConstructImuPriorConstraints(H, b);
    y0 += y0_imu;
  },"imu constraint");

  //! GNStep用于执行Gauss-Newton步进优化，它是实现优化算法的核心
  GNStep(sensor_measurement, H, b, y0, delta_x);

  return true;
}

//梯度下降法（Gradient Descent，GD）的单步迭代过程
bool LIO::GNStep(const SensorMeasurement& sensor_measurement,
                 Eigen::Matrix<double, 15, 15>& H,
                 Eigen::Matrix<double, 15, 1>& b,
                 const double y0,
                 Eigen::Matrix<double, 15, 1>& delta_x) 
{
  timer.Evaluate([&, this]() 
  {
    Eigen::Matrix<double, 15, 1> dir = -H.inverse() * b;

    State new_state;
    delta_x = dir;
    CorrectState(curr_state_, delta_x, new_state);
    curr_state_ = new_state;

    final_hessian_ = H;
  },"gn step");

  return true;
}

//! YEL：非常关键的函数，构建使用Wasserstein距离的GICP约束（使用Cauchy）
//前4次迭代：将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
//4次以后迭代：根据前三次生成的对应关系，更新trans_mean_A，与对应的mean_B计算残差项
double LIO::ConstructWassersteinGICPConstraints(Eigen::Matrix<double, 15, 15>& H, Eigen::Matrix<double, 15, 1>& b) 
{
  my_result_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  Eigen::Matrix<double, 8, 6> result_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  Eigen::Matrix<double, 8, 6> result_matrix_max_min = Eigen::Matrix<double, 8, 6>::Zero();
  Eigen::Matrix<double, 8, 6> init_matrix = Eigen::Matrix<double, 8, 6>::Zero();

  /*--------------------------------------4次迭代以后纯优化---------------------------------*/
  size_t cov_match = 0; size_t raw_jac_match = 0;  size_t jac_match = 0; size_t maxZ_match = 0;
  size_t low_score_A = 0;  size_t low_score_B = 0;  size_t low_score_C = 0;
  if (need_converge_) 
  {
    result_matrix = tbb::parallel_reduce(tbb::blocked_range<size_t>(0, Max_Group_second.size()),init_matrix,[&, this](tbb::blocked_range<size_t> r,Eigen::Matrix<double, 8, 6> local_result) 
    {
      //! 迭代每一组匹配
      for (size_t i = r.begin(); i < r.end(); ++i) 
      {
        Max_Group_mutex.lock();
        auto Max_Group_C = Max_Group_second[i];
        Max_Group_mutex.unlock();

        //! TODO::如果该组匹配在上次迭代后发现不再有效，则跳过
        if(!Max_Group_C->valid)  continue;

        //! 将mean_A转换到世界坐标系下，得到trans_mean_A
        Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * Max_Group_C->mean_A + curr_state_.pose.block<3, 1>(0, 3);
        
        //! 初始化自适应权重gicp_gain_w为1.0
        double gicp_gain_w = 1.0;

        //! 第一种情况：Cov匹配成功
        if(Max_Group_C->match_mode == 1)
        {
          //! 协方差相差越大，权重越小！
          gicp_gain_w = exp(-0.1*fabs(Max_Group_C->tr_A + Max_Group_C->tr_B - 2.0 * Max_Group_C->tr_AB));
          cov_match++;

          if(Max_Group_C->score<0.001)
            low_score_A++;

        }

        //! 第二种情况：二值匹配成功
        else if(Max_Group_C->match_mode == 2)
        {
          raw_jac_match++;

          //! 由于优化之后位置发生了改变，体素划分发生了改变，则二值特征cloud_cov_ptr_->points[Max_Group_second[i]->current_index].features也就发生了改变，因此重新计算相似度得分
          //int hamming_distance = ( cloud_cov_ptr_->points[Max_Group_C->current_index].features ^ Max_Group_C->B_map_features).count();
          //float similarity = 1.0 - static_cast<double>(hamming_distance) /  Max_Group_C->B_map_features.size();
          auto current_A_feature = cloud_cov_ptr_->points[Max_Group_C->current_index].features;
          double jaccard_distance = static_cast<double>((current_A_feature & Max_Group_C->B_map_features).count()) / (Max_Group_C->B_map_features | current_A_feature).count();
          
          //! 更新体素之后，jaccard距离不满足要求，重新进行匹配
          if(jaccard_distance<0.8)
          {
            bool refind = false;
            for (size_t search_idx = 0; search_idx < 7; ++search_idx) 
            {
              //! 匹配地图中的B点
              Eigen::Vector3d nearby_point = trans_mean_A + voxel_map_ptr_->delta_P_[search_idx];
              size_t hash_idx_B = voxel_map_ptr_->ComputeHashIndex(nearby_point);
              std::bitset<27> B_map_features;    
              bool is_valid_ = false;     
              Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
              Eigen::Matrix3d cov_B = Eigen::Matrix3d::Zero();
              bool cov_success = voxel_map_ptr_->GetCentroidAndCovariance(hash_idx_B, mean_B, cov_B, B_map_features, is_valid_);
              
              //! 没有找到点，或者不在同一个grid中,跳过
              if(!cov_success || !voxel_map_ptr_->IsSameGrid(nearby_point, mean_B)) continue;

              //hamming_distance = ( cloud_cov_ptr_->points[Max_Group_C->current_index].features ^ B_map_features).count();
              //similarity = 1.0 - static_cast<double>(hamming_distance) / B_map_features.size();
              jaccard_distance =static_cast<double>((current_A_feature & B_map_features).count()) / (B_map_features | current_A_feature).count();
              
              //! 重新搜索成功
              if(jaccard_distance>0.8)
              {
                refind = true;
                jac_match++;
                gicp_gain_w  = jaccard_distance;
                Max_Group_C->mean_B = mean_B;
                if(Max_Group_C->score<0.001)
                  low_score_B++;
                break;
              }
            }
            //! 遍历7个临近点，还是没有找到，就算了
            if(!refind)
            {
              Max_Group_C->valid = false;
              continue;
            }
          }
          else 
          {
            jac_match++;
            gicp_gain_w  = jaccard_distance;

            if(Max_Group_C->score<0.001)
              low_score_B++;
          }
        }

        //! 第三种情况：经过优化之后，检查还是不是最高点
        else if(Max_Group_C->match_mode == 3)
        {
          double temp_z{0};  bool AmaxZ = false; bool AminZ = false; 
          size_t hash_idx_A = fast_voxel_grid_ptr_->ComputeHashIndex(trans_mean_A);
          size_t hash_idx_A_z = fast_voxel_grid_ptr_->ComputeMaxZHashIndex(Max_Group_C->mean_A,curr_state_.pose.block<3, 3>(0, 0), curr_state_.pose.block<3, 1>(0, 3),temp_z);
          
          MyAccessor accessor;
          if (fast_voxel_grid_ptr_->voxel_map_MaxZ_ptr_->find(accessor, hash_idx_A_z)) 
          {
            if(hash_idx_A == accessor->second->maxZ_hash_index_)
              AmaxZ = true;
            else if(hash_idx_A == accessor->second->minZ_hash_index_)
              AminZ = true;
          }

          //! 如果A点不再是最高点或者最低点，则跳过
          if(!AmaxZ && !AminZ)
          {
            Max_Group_C->valid = false;
            continue;
          }
          maxZ_match++;

          //! 计算自适应权重
          gicp_gain_w =  10.0 * exp(-1.0/Max_Group_C->variance); //25*

          if(Max_Group_C->score<0.001)
            low_score_C++;
        }

        //TODO::很多score是0啊！

        gicp_gain_w = gicp_gain_w* Max_Group_C->score;
        //gicp_gain_w =  Max_Group_C->score;
        //LOG(INFO)<<"Max_Group_C->score: "<<Max_Group_C->score;

        //! 鲁棒损失函数计算
        Eigen::Vector3d error = Max_Group_C->mean_B - trans_mean_A;
        Eigen::Matrix3d mahalanobis = Max_Group_C->mahalanobis;    //mahalanobis由前4次迭代得到
        double cost_function = error.transpose() * mahalanobis * error;

        if (config_.enable_outlier_rejection && cost_function > 7.815) 
        {
          if(Max_Group_C->match_mode==1) cov_match--;
          if(Max_Group_C->match_mode==2) jac_match--;
          if(Max_Group_C->match_mode==3) maxZ_match--;
          Max_Group_C->valid = false;
          continue;
        }

        //! 根据cost_function的大小计算rho
        Eigen::Vector3d rho;
        CauchyLossFunction(cost_function, 10.0, rho);

        local_result(7, 0) += config_.gicp_constraint_gain * gicp_gain_w * rho[0];

        //! 误差相对于状态量的雅可比矩阵
        Eigen::Matrix<double, 3, 6> dres_dx = Eigen::Matrix<double, 3, 6>::Zero();

        //! 误差相对于旋转的雅可比矩阵
        dres_dx.block<3, 3>(0, 0) = curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(Max_Group_second[i]->mean_A);

        //! 误差相对于位置的雅可比矩阵
        dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

        Eigen::Matrix3d robust_information_matrix = config_.gicp_constraint_gain * gicp_gain_w * (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error * error.transpose() * mahalanobis);
        
        local_result.block(0, 0, 6, 6) += dres_dx.transpose() * robust_information_matrix * dres_dx;
        local_result.block(6, 0, 1, 6) += (config_.gicp_constraint_gain * gicp_gain_w * rho[1] * dres_dx.transpose() * mahalanobis * error).transpose();
      }
      return local_result;
    },[](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) {return x + y;});

      LOG(INFO)<<"low_score: "<< low_score_A<<"  "<<low_score_B<<"   "<<low_score_C;

      //! 计算H和b矩阵
      H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
      b.block<6, 1>(IndexErrorOri, 0) +=result_matrix.block<1, 6>(6, 0).transpose();
      
      LOG(INFO)<<"match(cov vs jac vs maxZ): "<< cov_match << "  "<<raw_jac_match<< "  "<<jac_match<<"  "<<maxZ_match;
      if(cov_match==0) LOG(FATAL)<< "cov_match==0 So Error";

      return result_matrix(7, 0);
  }

  /*****----4次迭代之前，建立数据关联----*****/
  //! 重新建立当前帧点云的voxel,计算二值特征
  fast_voxel_grid_ptr_->Filter(filtered_cloud_ptr, cloud_DS_ptr_, cloud_cov_ptr_,curr_state_.pose.block<3,3>(0,0), curr_state_.pose.block<3,1>(0,3));

  size_t delta_p_size = voxel_map_ptr_->delta_P_.size();

  //! 获取当前点云中点的数量
  size_t N = cloud_cov_ptr_->size();

  //清空存储点云与地图匹配结果的数组
  correspondences_array_.clear();

  size_t invalid_point = 0;  size_t valid_point = 0;  size_t dis_invalid_point = 0;  size_t invalid_point_chi2_error=0;
  size_t cov_valid = 0;  size_t neiber26=0;  size_t Match_Max_Z=0; size_t Match_Min_Z=0; size_t temp_invalid_cov =0;
  voxel_map_ptr_->invalid_point1 =0;  voxel_map_ptr_->invalid_point2 =0;

  //! 开始进行匹配
  std::vector<bool> v_match_valid(N, false);
  result_matrix = tbb::parallel_reduce(tbb::blocked_range<size_t>(0, N),init_matrix,[&, this](tbb::blocked_range<size_t> r,Eigen::Matrix<double, 8, 6> local_result) 
  {
    //! 将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
    for (size_t i = r.begin(); i < r.end(); ++i) 
    {
      //! 注意：这里的points[i]指的是对应的体素质心点。针对点云中的每个体素质心点，提取其协方差并转换到当前状态的世界坐标系中
      const PointCovType& point_cov = cloud_cov_ptr_->points[i];
      const Eigen::Vector3d mean_A = point_cov.getVector3fMap().cast<double>();
      const Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * mean_A + curr_state_.pose.block<3, 1>(0, 3);

      Eigen::Matrix3d cov_A;
      cov_A << point_cov.cov[0], point_cov.cov[1], point_cov.cov[2],
               point_cov.cov[1], point_cov.cov[3], point_cov.cov[4],
               point_cov.cov[2], point_cov.cov[4], point_cov.cov[5];

      size_t hash_idx_A = fast_voxel_grid_ptr_->ComputeHashIndex(trans_mean_A);

      std::shared_ptr<Correspondence> corr_ptr = std::make_shared<Correspondence>();

      Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
      Eigen::Matrix3d cov_B = Eigen::Matrix3d::Zero();

      //对于每个转换的点，寻找其在体素地图中邻近(7个)的点，并获取其对应的质心和协方差。匹配过程：cov匹配、二值描述子匹配、最高点匹配
      for (size_t search_idx = 0; search_idx < delta_p_size; ++search_idx) 
      {
        size_t match_mode = 0;
        
        //匹配地图中的B点
        Eigen::Vector3d nearby_point = trans_mean_A + voxel_map_ptr_->delta_P_[search_idx];
        size_t hash_idx_B = voxel_map_ptr_->ComputeHashIndex(nearby_point);
        std::bitset<27> B_map_features;    
        bool is_valid_ = false;
        bool cov_success = voxel_map_ptr_->GetCentroidAndCovariance(hash_idx_B, mean_B, cov_B,B_map_features,is_valid_);
        
        //没有找到点，或者不在同一个grid中
        if(!cov_success || !voxel_map_ptr_->IsSameGrid(nearby_point, mean_B)) continue;
 
        //海明距离
        //int hamming_distance = ( cloud_cov_ptr_->points[i].features ^ B_map_features).count();
        //double similarity = 1.0 - static_cast<double>(hamming_distance) / B_map_features.size();
        double similarity  = 0.0;
        Eigen::Matrix3d mahalanobis = Eigen::Matrix3d::Identity();

        //if(cov_A.trace()<0.001) is_valid_ = false;
        
        //! Step1：cov匹配成功
        if(is_valid_)
        {
          //if(similarity<0.3)
          //{
          //  temp_invalid_cov++;
            //continue;
          //}
          
          cov_valid++;
          match_mode = 1;
        }

        //!Step2:二值描述子匹配
        if(!is_valid_)
        {
          double jaccard_distance =static_cast<double>((cloud_cov_ptr_->points[i].features & B_map_features).count()) / (B_map_features | cloud_cov_ptr_->points[i].features).count();
          //Debug
          if(jaccard_distance>0.8)
          {
            match_mode = 2;
            similarity = jaccard_distance;
          }
        }

        //!Step3:最高点最低点匹配 
        double variance = 0.0;
        if(match_mode==0)
        {
          double temp_z{0};  
          bool AmaxZ = false;  bool AminZ = false; 
          //size_t hash_idx_A = fast_voxel_grid_ptr_->ComputeHashIndex(trans_mean_A);
          size_t hash_idx_A_z = fast_voxel_grid_ptr_->ComputeMaxZHashIndex(mean_A,curr_state_.pose.block<3, 3>(0, 0), curr_state_.pose.block<3, 1>(0, 3),temp_z);

          size_t cur_grid_x;size_t cur_grid_y;
          fast_voxel_grid_ptr_->GetGridXY(mean_A,curr_state_.pose.block<3, 3>(0, 0), curr_state_.pose.block<3, 1>(0, 3),cur_grid_x,cur_grid_y);
          MyGrid cur_grid;
          cur_grid.x = cur_grid_x; cur_grid.y = cur_grid_y;

          MyAccessor A_accessor;
          bool max_A_Z=true;  bool min_A_Z=true;//A是否为周围的最高点\最低点
          
          if (fast_voxel_grid_ptr_->voxel_map_MaxZ_ptr_->find(A_accessor, hash_idx_A_z)) 
          {
            //A是当前体素投影中的最高点
            if(hash_idx_A == A_accessor->second->maxZ_hash_index_)
            {
              int n=0;
              double mean=0.0;
              std::vector<double> v_z;
              //判断A点是否是周围的最高点
              for(int near_i=-1; near_i<=1; ++near_i)
              {
                for(int near_j=-1; near_j<=1;++near_j)
                {
                  MyGrid near_grid; near_grid.x = cur_grid_x + near_i; near_grid.y = cur_grid_y + near_j;
                  auto it = fast_voxel_grid_ptr_->max_z_map_.find(near_grid);
                  if (it != fast_voxel_grid_ptr_->max_z_map_.end()) 
                  {
                    v_z.push_back(it->second);
                    mean += it->second;
                    n++;
                  }
                  else
                    continue;

                  //跳过当前点
                  /*if(near_j==0 && near_i==0) continue;

                  //A不是周围最高点
                  if(it->second > fast_voxel_grid_ptr_->max_z_map_[cur_grid])
                  {
                    max_A_Z = false;
                    break;
                  }*/
                }
                //A点附近有比A点高的，那就不进行最高点匹配了
                if(!max_A_Z)  break;
              }

              //如果是最高点，计算平均值和协方差
              if(max_A_Z)
              {
                if(n!=0)  mean = mean/n;

                for (double value : v_z) 
                {
                    variance += (value - mean) * (value - mean);
                }
                if(n>1) variance = variance/(n-1);
              }

              AmaxZ = max_A_Z;
            } 

            //A是当前体素投影中的最低点
            if(hash_idx_A == A_accessor->second->minZ_hash_index_)
            {             
              //A还是周围的最低点
              int n=0;
              double mean=0.0;
              std::vector<double> v_z;
              for(int near_i=-1; near_i<=1; ++near_i)
              {
                for(int near_j=-1; near_j<=1;++near_j)
                {
                  MyGrid near_grid; near_grid.x = cur_grid_x + near_i; near_grid.y = cur_grid_y + near_j;
                  auto it = fast_voxel_grid_ptr_->min_z_map_.find(near_grid);
                  if (it != fast_voxel_grid_ptr_->min_z_map_.end()) 
                  {
                    v_z.push_back(it->second);
                    mean += it->second;
                    n++;
                  }
                  else
                    continue;

                  /*if(near_j==0 && near_i==0) continue;

                  //A不是周围最高点
                  if(it->second < fast_voxel_grid_ptr_->min_z_map_[cur_grid])
                  {
                    min_A_Z = false;
                    break;
                  }*/
                }
                //A点附近有比A点低的，那就不进行最低点匹配了
                if(!min_A_Z)  break;
              }

              if(min_A_Z)
              {
                if(n!=0) mean = mean/n;
                
                for (double value : v_z) 
                {
                    variance += (value - mean) * (value - mean);
                }

                if(n>1) variance = variance/(n-1);
              }
              AminZ = min_A_Z;
            } 
          }

          //即不是最高点也不是最低点，则不进行匹配
          if(!AmaxZ && !AminZ)
            continue;

          //对于每个转换的点，寻找其在体素地图中邻近(7个)的点，并获取其对应的质心和协方差。匹配过程：cov匹配、二值描述子匹配、最高点匹配
          for (size_t search_idx = 0; search_idx < delta_p_size; ++search_idx) 
          {
            //匹配地图中的B点
            Eigen::Vector3d nearby_point = trans_mean_A + voxel_map_ptr_->delta_P_[search_idx];
            size_t hash_idx_B = voxel_map_ptr_->ComputeHashIndex(nearby_point);
            std::bitset<27> B_map_features;    
            bool is_valid_ = false;
            bool cov_success = voxel_map_ptr_->GetCentroidAndCovariance(hash_idx_B, mean_B, cov_B,B_map_features,is_valid_);
            
            //没有找到点，或者不在同一个grid中
            if(!cov_success || !voxel_map_ptr_->IsSameGrid(nearby_point, mean_B)) continue;

            size_t B_hash_Z  = voxel_map_ptr_->ComputeMaxZHashIndex(mean_B); //z=0的hash值

            MyAccessor B_accessor;
            if (voxel_map_ptr_->voxel_MaxZ_map_->find(B_accessor, B_hash_Z)) 
            {
              //如果满足最高点约束
              if(AmaxZ && (hash_idx_B == B_accessor->second->maxZ_hash_index_))
              {
                match_mode =3;
                break;
              }
              else if(AminZ && (hash_idx_B == B_accessor->second->minZ_hash_index_))
              {
                match_mode =3;
                break;
              }
              else //当前的B点不是地图中的最高点
                continue;
            }
            else //地图中没有这个点
              continue;
          }

          //如果没有找到匹配点
          if(match_mode==0) continue;
        }

        double gicp_gain_w = 1.0;

        //! YEL--①第一种情况：cov匹配成功
        if(match_mode==1)
        {
          //计算马氏距离以评估当前点云点与找到的对应点之间的误差
          mahalanobis = (cov_B + curr_state_.pose.block<3, 3>(0, 0) * cov_A *curr_state_.pose.block<3, 3>(0, 0).transpose() + Eigen::Matrix3d::Identity() * 1e-3).inverse();
          //! YEL--计算关于协方差矩阵迹的数据
          corr_ptr->tr_A = cov_A.trace();
          corr_ptr->tr_B = cov_B.trace();
          Eigen::Matrix3d cov_AB = cov_A*cov_B;
          corr_ptr->tr_AB = MatrixSqrt(MatrixSqrt(cov_B, cov_B.rows())*cov_A*MatrixSqrt(cov_B, cov_B.rows()), cov_B.rows()).trace();
          gicp_gain_w = exp(-0.1*fabs(corr_ptr->tr_A + corr_ptr->tr_B - 2.0*corr_ptr->tr_AB));
          //gicp_gain_w = 1.0;
        }

        //! YEL--②第二种情况：二值描述子匹配成功
        if(match_mode==2)
        {
          mahalanobis = Eigen::Matrix3d::Identity();//(Eigen::Matrix3d::Identity()*similarity).inverse();
          gicp_gain_w = similarity;
          neiber26++;
        }

        //! YEL--③第三种情况：最高点匹配成功
        if(match_mode==3)
        {
          mahalanobis = Eigen::Matrix3d::Identity();
          gicp_gain_w =10.0 * exp(-1.0/variance); //exp(-1.0*(1.0/variance));
          Match_Max_Z++;
        }

        Eigen::Vector3d error = mean_B - trans_mean_A;

        //! 3次迭代之后，即第4次时，进行卡方检验，剔除外点，保证匹配点的可靠性
        double chi2_error = error.transpose() * mahalanobis * error;
        if (config_.enable_outlier_rejection && iter_num_ > 2 && chi2_error > 7.815) 
        {
          invalid_point_chi2_error++;
          if(match_mode==1) cov_valid--;
          if(match_mode==2) neiber26--;
          if(match_mode==3) Match_Max_Z--;
          continue;
        }

        //! 损失函数
        double cost_function = chi2_error;

        Eigen::Vector3d rho;
        //! (尝试不使用Cauchy损失函数)接着调用CauchyLossFunction，这是一个鲁棒损失函数，用于减小异常值对优化结果的影响。该函数返回的rho向量通常包含了代价函数的各类信息（如残差和对应的梯度）
        CauchyLossFunction(cost_function, 10.0, rho);

        //! 根据GICP约束增益config_.gicp_constraint_gain和rho[0]的值，更新局部结果
        local_result(7, 0) += config_.gicp_constraint_gain * gicp_gain_w * rho[0];

        //! 存储残差对于状态变量的偏导数
        Eigen::Matrix<double, 3, 6> dres_dx = Eigen::Matrix<double, 3, 6>::Zero();

        //! 通过当前状态的姿态curr_state_.pose和mean_A（相对于当前点云的值），计算旋转的偏导数
        dres_dx.block<3, 3>(0, 0) = curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(mean_A);

        //! 将偏导数的最后3列设置为负单位矩阵，表示位置对误差的影响
        dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

        //! 创建了一个鲁棒信息矩阵，使用了mahalanobis（马氏距离的协方差）以及之前计算的rho，将这些结合以确保优化具有鲁棒性
        Eigen::Matrix3d robust_information_matrix = config_.gicp_constraint_gain * gicp_gain_w * (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error * error.transpose() * mahalanobis);

        //! 使用计算得到的偏导数和鲁棒信息矩阵更新local_result，将偏导数的转置与鲁棒信息矩阵乘积的结果加到局部结果
        local_result.block(0, 0, 6, 6) += dres_dx.transpose() * robust_information_matrix * dres_dx;

        //! 将通过加权计算的偏导数、马氏距离和误差（error）的乘积更新局部结果的另一部分
        local_result.block(6, 0, 1, 6) += (config_.gicp_constraint_gain * gicp_gain_w * rho[1] * dres_dx.transpose() * mahalanobis * error). transpose();

        //! 存储当前点云点与找到的对应点之间的对应关系
        corr_ptr->mean_A = mean_A;
        corr_ptr->mean_B = mean_B;
        corr_ptr->mahalanobis = mahalanobis;
        corr_ptr->is_valid = is_valid_;
        corr_ptr->current_index = i;
        corr_ptr->A_cur_features = cloud_cov_ptr_->points[i].features;
        corr_ptr->B_map_features = B_map_features;
        corr_ptr->match_mode = match_mode;
        corr_ptr->valid = true;
        corr_ptr->local_result = local_result;
        corr_ptr->score = 1.0;
        v_match_valid[i] = true;

        //! 存储对应关系
        correspondences_array_.emplace_back(corr_ptr);

        break;
      }//每个点只要找到一个对应，就不再对7个临近的grid进行搜索 
    }
    return local_result;},[](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) 
    {
      return x + y;
    });
/*
  //匹配最高最低点
  init_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  result_matrix_max_min = tbb::parallel_reduce(tbb::blocked_range<size_t>(0, N),init_matrix,[&, this](tbb::blocked_range<size_t> r,Eigen::Matrix<double, 8, 6> local_result) 
  {
    //! 将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
    for (size_t i = r.begin(); i < r.end(); ++i) 
    {
      //已经匹配过的就不再匹配
      if(v_match_valid[i]) continue;

      //! 注意：这里的points[i]指的是对应的体素质心点。针对点云中的每个体素质心点，提取其协方差并转换到当前状态的世界坐标系中
      const PointCovType& point_cov = cloud_cov_ptr_->points[i];
      const Eigen::Vector3d mean_A = point_cov.getVector3fMap().cast<double>();
      const Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * mean_A + curr_state_.pose.block<3, 1>(0, 3);

      Eigen::Matrix3d cov_A;
      cov_A << point_cov.cov[0], point_cov.cov[1], point_cov.cov[2],
               point_cov.cov[1], point_cov.cov[3], point_cov.cov[4],
               point_cov.cov[2], point_cov.cov[4], point_cov.cov[5];

      double temp_z{0};  
      bool AmaxZ = false;  bool AminZ = false; 
      size_t hash_idx_A = fast_voxel_grid_ptr_->ComputeHashIndex(trans_mean_A);
      size_t hash_idx_A_z = fast_voxel_grid_ptr_->ComputeMaxZHashIndex(mean_A,curr_state_.pose.block<3, 3>(0, 0), curr_state_.pose.block<3, 1>(0, 3),temp_z);

      size_t cur_grid_x;size_t cur_grid_y;
      fast_voxel_grid_ptr_->GetGridXY(mean_A,curr_state_.pose.block<3, 3>(0, 0), curr_state_.pose.block<3, 1>(0, 3),cur_grid_x,cur_grid_y);
      MyGrid cur_grid;
      cur_grid.x = cur_grid_x; cur_grid.y = cur_grid_y;

      MyAccessor A_accessor;
      //A是否为周围的最高点\最低点
      bool max_A_Z=true;  bool min_A_Z=true;
      double variance = 0.0;
      if (fast_voxel_grid_ptr_->voxel_map_MaxZ_ptr_->find(A_accessor, hash_idx_A_z)) 
      {
        //A是当前体素投影中的最高点
        if(hash_idx_A == A_accessor->second->maxZ_hash_index_)
        {
          if(fabs(fast_voxel_grid_ptr_->max_z_map_[cur_grid]-A_accessor->second->max_z)>0.01)
             LOG(ERROR)<<"max----"<<fast_voxel_grid_ptr_->max_z_map_[cur_grid]<<A_accessor->second->max_z;

          int n=0;
          double mean=0.0;
          std::vector<double> v_z;
          //判断A点是否是周围的最高点
          for(int near_i=-1; near_i<=1; ++near_i)
          {
            for(int near_j=-1; near_j<=1;++near_j)
            {
              MyGrid near_grid; near_grid.x = cur_grid_x + near_i; near_grid.y = cur_grid_y + near_j;
              auto it = fast_voxel_grid_ptr_->max_z_map_.find(near_grid);
              if (it != fast_voxel_grid_ptr_->max_z_map_.end()) 
              {
                v_z.push_back(it->second);
                mean += it->second;
                n++;
              }
              else
                continue;

              //跳过当前点
              if(near_j==0 && near_i==0) continue;

              //A不是周围最高点
              if(it->second > fast_voxel_grid_ptr_->max_z_map_[cur_grid])
              {
                max_A_Z = false;
                break;
              }
            }
            //A点附近有比A点高的，那就不进行最高点匹配了
            if(!max_A_Z)  break;
          }

          //如果是最高点，计算平均值和协方差
          if(max_A_Z)
          {
            if(n!=0)  mean = mean/n;
            for (double value : v_z) 
            {
                variance += (value - mean) * (value - mean);
            }
            if(n>1) variance = variance/(n-1);
          }

          AmaxZ = max_A_Z;
        } 

        //A是当前体素投影中的最低点
        if(hash_idx_A == A_accessor->second->minZ_hash_index_)
        {
          if(fabs(fast_voxel_grid_ptr_->min_z_map_[cur_grid]-A_accessor->second->min_z)>0.01)
            LOG(ERROR)<<"min----"<<fast_voxel_grid_ptr_->min_z_map_[cur_grid]<<A_accessor->second->min_z;
            
          //A还是周围的最低点
          int n=0;
          double mean=0.0;
          std::vector<double> v_z;
          for(int near_i=-1; near_i<=1; ++near_i)
          {
            for(int near_j=-1; near_j<=1;++near_j)
            {
              MyGrid near_grid; near_grid.x = cur_grid_x + near_i; near_grid.y = cur_grid_y + near_j;
              auto it = fast_voxel_grid_ptr_->min_z_map_.find(near_grid);
              if (it != fast_voxel_grid_ptr_->min_z_map_.end()) 
              {
                v_z.push_back(it->second);
                mean += it->second;
                n++;
              }
              else
                continue;

              if(near_j==0 && near_i==0) continue;

              //A不是周围最高点
              if(it->second < fast_voxel_grid_ptr_->min_z_map_[cur_grid])
              {
                min_A_Z = false;
                break;
              }
            }
            //A点附近有比A点低的，那就不进行最低点匹配了
            if(!min_A_Z)  break;
          }

          if(min_A_Z)
          {
            if(n!=0) mean = mean/n;
            
            for (double value : v_z) 
            {
                variance += (value - mean) * (value - mean);
            }

            if(n>1) variance = variance/(n-1);
          }

          AminZ = min_A_Z;
        } 
      }

      //即不是最高点也不是最低点，则不进行匹配
      if(!AmaxZ || !AminZ)
        continue;

      std::shared_ptr<Correspondence> corr_ptr = std::make_shared<Correspondence>();

      Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
      Eigen::Matrix3d cov_B = Eigen::Matrix3d::Zero();

      //对于每个转换的点，寻找其在体素地图中邻近(7个)的点，并获取其对应的质心和协方差。匹配过程：cov匹配、二值描述子匹配、最高点匹配
      for (size_t search_idx = 0; search_idx < delta_p_size; ++search_idx) 
      {
        size_t match_mode = 0;

        //匹配地图中的B点
        Eigen::Vector3d nearby_point = trans_mean_A + voxel_map_ptr_->delta_P_[search_idx];
        size_t hash_idx_B = voxel_map_ptr_->ComputeHashIndex(nearby_point);
        std::bitset<27> B_map_features;    
        bool is_valid_ = false;
        bool cov_success = voxel_map_ptr_->GetCentroidAndCovariance(hash_idx_B, mean_B, cov_B,B_map_features,is_valid_);
        //没有找到点，或者不在同一个grid中
        if(!cov_success || !voxel_map_ptr_->IsSameGrid(nearby_point, mean_B))
        {
          continue;
        }

        double similarity =1.0;
        double gicp_gain_w = 1.0;
        Eigen::Matrix3d mahalanobis = Eigen::Matrix3d::Identity();

        //!Step3:最高点匹配
        if(AmaxZ || AminZ)
        {
          size_t B_hash_Z  = voxel_map_ptr_->ComputeMaxZHashIndex(mean_B); //z=0的hash值

          MyAccessor B_accessor;
          if (voxel_map_ptr_->voxel_MaxZ_map_->find(B_accessor, B_hash_Z)) 
          {
            //如果满足最高点约束
            if(AmaxZ && (hash_idx_B == B_accessor->second->maxZ_hash_index_))
              match_mode =3;
            else if(AminZ && (hash_idx_B == B_accessor->second->minZ_hash_index_))
              match_mode =3;
            else //当前的B点不是地图中的最高点
              continue;
          }
          else //地图中没有这个点
            continue;
        }
        else//A也不是最高点
          continue;

        //如果没有找到匹配点
        if(match_mode==0)
        {
          LOG(ERROR)<< "match_mode is 0";
        }

        //! YEL--③第三种情况：最高点匹配成功
        if(match_mode==3)
        {
          mahalanobis = Eigen::Matrix3d::Identity();
          gicp_gain_w = 25.0 * exp(-1.0*(1.0/variance)); //exp(-1.0*(1.0/variance));
          Match_Max_Z++;
        }

        Eigen::Vector3d error = mean_B - trans_mean_A;

        //! 3次迭代之后，即第4次时，进行卡方检验，剔除外点，保证匹配点的可靠性
        double chi2_error = error.transpose() * mahalanobis * error;
        if (config_.enable_outlier_rejection && iter_num_ > 2 && chi2_error > 7.815) 
        {
          invalid_point_chi2_error++;
          if(match_mode==3) Match_Max_Z--;
          continue;
        }

        //! 损失函数
        double cost_function = chi2_error;

        Eigen::Vector3d rho;
        //! (尝试不使用Cauchy损失函数)接着调用CauchyLossFunction，这是一个鲁棒损失函数，用于减小异常值对优化结果的影响。该函数返回的rho向量通常包含了代价函数的各类信息（如残差和对应的梯度）
        CauchyLossFunction(cost_function, 10.0, rho);

        //! 根据GICP约束增益config_.gicp_constraint_gain和rho[0]的值，更新局部结果
        local_result(7, 0) += config_.gicp_constraint_gain * gicp_gain_w * rho[0];

        //! 存储残差对于状态变量的偏导数
        Eigen::Matrix<double, 3, 6> dres_dx = Eigen::Matrix<double, 3, 6>::Zero();

        //! 通过当前状态的姿态curr_state_.pose和mean_A（相对于当前点云的值），计算旋转的偏导数
        dres_dx.block<3, 3>(0, 0) = curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(mean_A);

        //! 将偏导数的最后3列设置为负单位矩阵，表示位置对误差的影响
        dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

        //! 创建了一个鲁棒信息矩阵，使用了mahalanobis（马氏距离的协方差）以及之前计算的rho，将这些结合以确保优化具有鲁棒性
        Eigen::Matrix3d robust_information_matrix = config_.gicp_constraint_gain * gicp_gain_w * (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error * error.transpose() * mahalanobis);

        //! 使用计算得到的偏导数和鲁棒信息矩阵更新local_result，将偏导数的转置与鲁棒信息矩阵乘积的结果加到局部结果
        local_result.block(0, 0, 6, 6) += dres_dx.transpose() * robust_information_matrix * dres_dx;

        //! 将通过加权计算的偏导数、马氏距离和误差（error）的乘积更新局部结果的另一部分
        local_result.block(6, 0, 1, 6) += (config_.gicp_constraint_gain * gicp_gain_w * rho[1] * dres_dx.transpose() * mahalanobis * error). transpose();

        //! 存储当前点云点与找到的对应点之间的对应关系
        corr_ptr->mean_A = mean_A;
        corr_ptr->mean_B = mean_B;
        corr_ptr->mahalanobis = mahalanobis;
        corr_ptr->is_valid = is_valid_;
        corr_ptr->current_index = i;
        corr_ptr->A_cur_features = cloud_cov_ptr_->points[i].features;
        corr_ptr->B_map_features = B_map_features;
        corr_ptr->match_mode = match_mode;
        corr_ptr->valid = true;
        corr_ptr->variance = variance;
        corr_ptr->local_result = local_result;
        v_match_valid[i] = true;

        //! 存储对应关系
        correspondences_array_.emplace_back(corr_ptr);

        break;//每个点只要找到一个对应，就不再对7个临近的grid进行搜索 
      }
    }
    return local_result;},[](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) 
    {
      return x + y;
    });
*/
    //LOG (INFO) <<"匹配点数量："<< correspondences_array_.size()<<"  valid_point: " << valid_point <<"  invalid_point: " << invalid_point << " dis_invalid_point: " << dis_invalid_point;
    //LOG (INFO) <<"使用26作为描述子： "<<neiber26 <<"最高点匹配："<<Match_Max_Z;
    //LOG(INFO)<<"有效的cov:"<<cov_valid<<"被26剔除： "<<temp_invalid_cov;

    result_matrix =  result_matrix_max_min + result_matrix;
/************yel add start*********/
    if(iter_num_==3)
    {
      Max_Group_second.clear();
      if(correspondences_array_.size()<500)
      {
        Max_Group_second.swap(correspondences_array_);
        LOG(WARNING) << "Max_Group_second size<500: " << Max_Group_second.size();
        my_result_matrix = result_matrix;
      }
      else
      {
        Graphs.clear();
        SearchGroups();
      }
    }
    else
      my_result_matrix = result_matrix;

    effect_feat_num_ = correspondences_array_.size();
    H.block<6, 6>(IndexErrorOri, IndexErrorOri) += my_result_matrix.block<6, 6>(0, 0);
    b.block<6, 1>(IndexErrorOri, 0) += my_result_matrix.block<1, 6>(6, 0).transpose();

    return my_result_matrix(7, 0);
}

//! 非常关键的函数，构建GICP约束（使用Cauchy）
//前三次迭代：将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
//三次以后迭代：根据前三次生成的对应关系，更新trans_mean_A，与对应的mean_B计算残差项
double LIO::ConstructGICPConstraints(Eigen::Matrix<double, 15, 15>& H,
                                     Eigen::Matrix<double, 15, 1>& b) 
{
  Eigen::Matrix<double, 8, 6> result_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  Eigen::Matrix<double, 8, 6> init_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  //第三次迭代以后
  if (need_converge_) 
  {
    //定义了一个tbb::parallel_reduce对象，该对象可以并行地计算给定范围中的数据
    result_matrix = tbb::parallel_reduce(
        tbb::blocked_range<size_t>(0, correspondences_array_.size()),
        init_matrix,
        [&, this](tbb::blocked_range<size_t> r,
                  Eigen::Matrix<double, 8, 6> local_result) 
        {
          for (size_t i = r.begin(); i < r.end(); ++i) 
          {
            //将mean_A转换到世界坐标系下，得到trans_mean_A
            Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * correspondences_array_[i]->mean_A +curr_state_.pose.block<3, 1>(0, 3);
            //计算trans_mean_A和地图中mean_B之间的误差
            Eigen::Vector3d error = correspondences_array_[i]->mean_B - trans_mean_A;

            // without loss function
            // local_result(7, 0) += gicp_constraint_gain_ * error.transpose() *
            //                       correspondences_array_[i]->mahalanobis *
            //                       error;

            // // The residual takes the partial derivative of the state
            // Eigen::Matrix<double, 3, 6> dres_dx =
            //     Eigen::Matrix<double, 3, 6>::Zero();

            // // The residual takes the partial derivative of rotation
            // dres_dx.block<3, 3>(0, 0) =
            //     curr_state_.pose.block<3, 3>(0, 0) *
            //     Sophus::SO3d::hat(correspondences_array_[i]->mean_A);

            // // The residual takes the partial derivative of position
            // dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

            // local_result.block(0, 0, 6, 6) +=
            //     gicp_constraint_gain_ * dres_dx.transpose() *
            //     correspondences_array_[i]->mahalanobis * dres_dx;

            // local_result.block(6, 0, 1, 6) +=
            //     (gicp_constraint_gain_ * dres_dx.transpose() *
            //      correspondences_array_[i]->mahalanobis * error)
            //         .transpose();

            // 鲁棒损失函数计算
            Eigen::Matrix3d mahalanobis = correspondences_array_[i]->mahalanobis;
            double cost_function = error.transpose() * mahalanobis * error;
            Eigen::Vector3d rho;
            CauchyLossFunction(cost_function, 10.0, rho);

            local_result(7, 0) += config_.gicp_constraint_gain * rho[0];

            // 误差相对于状态量的雅可比矩阵
            Eigen::Matrix<double, 3, 6> dres_dx = Eigen::Matrix<double, 3, 6>::Zero();

            // 误差相对于旋转的雅可比矩阵
            dres_dx.block<3, 3>(0, 0) =
                curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(correspondences_array_[i]->mean_A);

            // 误差相对于位置的雅可比矩阵
            dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

            Eigen::Matrix3d robust_information_matrix = config_.gicp_constraint_gain * (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error * error.transpose() * mahalanobis);
            
            local_result.block(0, 0, 6, 6) += dres_dx.transpose() * robust_information_matrix * dres_dx;
            local_result.block(6, 0, 1, 6) += (config_.gicp_constraint_gain * rho[1] * dres_dx.transpose() * mahalanobis * error).transpose();
          }

          return local_result;
        },

        [](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) {return x + y;});

    H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
    b.block<6, 1>(IndexErrorOri, 0) +=result_matrix.block<1, 6>(6, 0).transpose();

    return result_matrix(7, 0);
  }

  //前三次迭代
  size_t delta_p_size = voxel_map_ptr_->delta_P_.size();
  //获取当前点云中点的数量
  size_t N = cloud_cov_ptr_->size();
  //清空存储点云与地图匹配结果的数组
  correspondences_array_.clear();
  //使用 Intel TBB 库进行并行处理，这样可以提高处理速度。循环将处理从 0 到 N 范围内的点
  result_matrix = tbb::parallel_reduce(
      tbb::blocked_range<size_t>(0, N),
      init_matrix,
      [&, this](tbb::blocked_range<size_t> r,
                Eigen::Matrix<double, 8, 6> local_result) 
     {
        //! 将点云中的点mean_A转换到世界坐标下，得到trans_mean_A，并根据delta_P_，在地图中寻找mean_B，计算mean_A和mean_B生成的残差项。2次迭代之后进行卡方检验，剔除外点
        for (size_t i = r.begin(); i < r.end(); ++i) 
        {
          //针对点云中的每个点，提取其协方差并转换到当前状态的世界坐标系中
          const PointCovType& point_cov = cloud_cov_ptr_->points[i];
          const Eigen::Vector3d mean_A = point_cov.getVector3fMap().cast<double>();
          const Eigen::Vector3d trans_mean_A = curr_state_.pose.block<3, 3>(0, 0) * mean_A + curr_state_.pose.block<3, 1>(0, 3);

          Eigen::Matrix3d cov_A;
          cov_A << point_cov.cov[0], point_cov.cov[1], point_cov.cov[2],
              point_cov.cov[1], point_cov.cov[3], point_cov.cov[4],
              point_cov.cov[2], point_cov.cov[4], point_cov.cov[5];

          Eigen::Vector3d mean_B = Eigen::Vector3d::Zero();
          Eigen::Matrix3d cov_B = Eigen::Matrix3d::Zero();
          //对于每个转换的点，寻找其在体素地图中邻近的点，并计算其对应的质心和协方差
          for (size_t i = 0; i < delta_p_size; ++i) 
          {
            Eigen::Vector3d nearby_point = trans_mean_A + voxel_map_ptr_->delta_P_[i];
            //通过 Hash 索引来加速查找，提高处理效率
            size_t hash_idx = voxel_map_ptr_->ComputeHashIndex(nearby_point);
            std::bitset<27> B_map_features; 
            bool is_valid_ = false;
            //if (voxel_map_ptr_->GetCentroidAndCovariance(hash_idx, mean_B, cov_B) && voxel_map_ptr_->IsSameGrid(nearby_point, mean_B)) 
            if (voxel_map_ptr_->GetCentroidAndCovariance(hash_idx, mean_B, cov_B,B_map_features,is_valid_) && voxel_map_ptr_->IsSameGrid(nearby_point, mean_B)) 
            {
              if(!is_valid_)
                continue;
              //计算马氏距离以评估当前点云点与找到的对应点之间的误差
              Eigen::Matrix3d mahalanobis = (cov_B + curr_state_.pose.block<3, 3>(0, 0) * cov_A *curr_state_.pose.block<3, 3>(0, 0).transpose() + Eigen::Matrix3d::Identity() * 1e-3).inverse();

              Eigen::Vector3d error = mean_B - trans_mean_A;
              //通过卡方检验剔除外点，保证匹配点的可靠性
              double chi2_error = error.transpose() * mahalanobis * error;
              //2次迭代之后进行卡方检验，剔除外点
              if (config_.enable_outlier_rejection) 
              {
                if (iter_num_ > 2 && chi2_error > 7.815) 
                {
                  continue;
                }
              }
              //生成当前点云与地图的对应，用于三次以后迭代的残差项计算
              //如果点是有效的，则将匹配结果存入 correspondences_array_
              std::shared_ptr<Correspondence> corr_ptr =
                  std::make_shared<Correspondence>();
              corr_ptr->mean_A = mean_A;
              corr_ptr->mean_B = mean_B;
              corr_ptr->mahalanobis = mahalanobis;
              correspondences_array_.emplace_back(corr_ptr);

              // without loss function
              // local_result(7, 0) += gicp_constraint_gain_ * chi2_error;

              // // The residual takes the partial derivative of the state
              // Eigen::Matrix<double, 3, 6> dres_dx =
              //     Eigen::Matrix<double, 3, 6>::Zero();

              // // The residual takes the partial derivative of rotation
              // dres_dx.block<3, 3>(0, 0) = curr_state_.pose.block<3, 3>(0, 0)
              // *
              //                             Sophus::SO3d::hat(mean_A);

              // // The residual takes the partial derivative of position
              // dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

              // local_result.block(0, 0, 6, 6) += gicp_constraint_gain_ *
              //                                   dres_dx.transpose() *
              //                                   mahalanobis * dres_dx;

              // local_result.block(6, 0, 1, 6) +=
              //     (gicp_constraint_gain_ * dres_dx.transpose() * mahalanobis
              //     *
              //      error)
              //         .transpose();

              // loss function
              double cost_function = chi2_error;
              Eigen::Vector3d rho;
              //接着调用CauchyLossFunction，这是一个鲁棒损失函数，用于减小异常值对优化结果的影响。该函数返回的rho向量通常包含了代价函数的各类信息（如残差和对应的梯度）
              CauchyLossFunction(cost_function, 10.0, rho);
              //根据GICP约束增益config_.gicp_constraint_gain和rho[0]的值，更新局部结果
              local_result(7, 0) += config_.gicp_constraint_gain * rho[0];

              //存储残差对于状态变量的偏导数
              Eigen::Matrix<double, 3, 6> dres_dx =
                  Eigen::Matrix<double, 3, 6>::Zero();

              //通过当前状态的姿态curr_state_.pose和mean_A（相对于当前点云的值），计算旋转的偏导数
              dres_dx.block<3, 3>(0, 0) = curr_state_.pose.block<3, 3>(0, 0) *
                                          Sophus::SO3d::hat(mean_A);

              //将偏导数的最后3列设置为负单位矩阵，表示位置对误差的影响
              dres_dx.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();

              //创建了一个鲁棒信息矩阵，使用了mahalanobis（马氏距离的协方差）以及之前计算的rho，将这些结合以确保优化具有鲁棒性
              Eigen::Matrix3d robust_information_matrix =
                  config_.gicp_constraint_gain *
                  (rho[1] * mahalanobis + 2.0 * rho[2] * mahalanobis * error *
                                              error.transpose() * mahalanobis);

              //使用计算得到的偏导数和鲁棒信息矩阵更新local_result，将偏导数的转置与鲁棒信息矩阵乘积的结果加到局部结果
              local_result.block(0, 0, 6, 6) +=
                  dres_dx.transpose() * robust_information_matrix * dres_dx;

              //将通过加权计算的偏导数、马氏距离和误差（error）的乘积更新局部结果的另一部分
              local_result.block(6, 0, 1, 6) +=
                  (config_.gicp_constraint_gain * rho[1] * dres_dx.transpose() *
                   mahalanobis * error)
                      .transpose();

              break;
            }
            else
            {
              //if(!voxel_map_ptr_->IsSameGrid(nearby_point, mean_B))
              //  LOG(ERROR)<<"Is not Same Grid";
            }
          }
        }

        return local_result;
      },
      [](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) {
        return x + y;
      });

  effect_feat_num_ = correspondences_array_.size();

  H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
  b.block<6, 1>(IndexErrorOri, 0) += result_matrix.block<1, 6>(6, 0).transpose();

  return result_matrix(7, 0);
}

//前三次迭代：KNN搜索最近点构建平面方程，计算点到平面距离，构建残差项
//后续迭代：根据前三次建立的点和平面对齐关系，计算点到平面距离，构建残差项
double LIO::ConstructPoint2PlaneConstraints(Eigen::Matrix<double, 15, 15>& H,
                                            Eigen::Matrix<double, 15, 1>& b) 
{
  Eigen::Matrix<double, 8, 6> result_matrix = Eigen::Matrix<double, 8, 6>::Zero();
  Eigen::Matrix<double, 8, 6> init_matrix = Eigen::Matrix<double, 8, 6>::Zero();

  // 迭代优化三次以后，跳过KNN搜索最近点来加速收敛
  if (need_converge_) 
  {
    result_matrix = tbb::parallel_reduce(
        tbb::blocked_range<size_t>(0, correspondences_array_.size()),
        init_matrix,
        [&, this](tbb::blocked_range<size_t> r,
                  Eigen::Matrix<double, 8, 6> local_result) 
        {
          //计算correspondences_array_中点到平面距离：首先将点mean_A转换到世界坐标系下，得到trans_pt，然后计算trans_pt到平面的距离error
          for (size_t i = r.begin(); i < r.end(); ++i) 
          {
            const Eigen::Vector3d trans_pt = curr_state_.pose.block<3, 3>(0, 0) *correspondences_array_[i]->mean_A + curr_state_.pose.block<3, 1>(0, 3);
            const Eigen::Vector4d& plane_coeff = correspondences_array_[i]->plane_coeff;

            double error = plane_coeff.head(3).dot(trans_pt) + plane_coeff(3, 0);

            //计算含增益的残差项
            local_result(7, 0) += config_.point2plane_constraint_gain * error * error;

            // The residual takes the partial derivative of the state
            Eigen::Matrix<double, 1, 6> dres_dx = Eigen::Matrix<double, 1, 6>::Zero();

            // The residual takes the partial derivative of rotation
            dres_dx.block<1, 3>(0, 0) = -plane_coeff.head(3).transpose() * curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(correspondences_array_[i]->mean_A);

            // The residual takes the partial derivative of position
            dres_dx.block<1, 3>(0, 3) = plane_coeff.head(3).transpose();

            local_result.block(0, 0, 6, 6) += config_.point2plane_constraint_gain * dres_dx.transpose() * dres_dx;

            local_result.block(6, 0, 1, 6) += config_.point2plane_constraint_gain * dres_dx * error;
          }

          return local_result;
        },
        [](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) { return x + y;});

    H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
    b.block<6, 1>(IndexErrorOri, 0) += result_matrix.block<1, 6>(6, 0).transpose();

    return result_matrix(7, 0);
  }

  size_t N = cloud_cov_ptr_->size();
  correspondences_array_.clear();
  result_matrix = tbb::parallel_reduce(tbb::blocked_range<size_t>(0, N),init_matrix,[&, this](tbb::blocked_range<size_t> r,Eigen::Matrix<double, 8, 6> local_result) 
      {
        for (size_t i = r.begin(); i < r.end(); ++i) 
        {
          //将点转换到世界坐标系下，得到p_w
          const Eigen::Vector3d p = cloud_cov_ptr_->points[i].getVector3fMap().cast<double>();
          const Eigen::Vector3d p_w = curr_state_.pose.block<3, 3>(0, 0) * p + curr_state_.pose.block<3, 1>(0, 3);

          std::vector<Eigen::Vector3d> nearest_points;
          nearest_points.reserve(10);
          //找到p_w的最近邻点，存入nearest_points
          voxel_map_ptr_->KNNByCondition(p_w, 5, 5.0, nearest_points);

          //计算nearest_points对应的平面参数，计算点到平面的距离，生成残差项
          Eigen::Vector4d plane_coeff;
          if (nearest_points.size() >= 3 && EstimatePlane(plane_coeff, nearest_points)) 
          {
            double error = plane_coeff.head(3).dot(p_w) + plane_coeff(3, 0);

            bool is_vaild = p.norm() > (81 * error * error);
            if (is_vaild) 
            {
              std::shared_ptr<Correspondence> corr_ptr =
                  std::make_shared<Correspondence>();
              corr_ptr->mean_A = p;
              corr_ptr->plane_coeff = plane_coeff;
              correspondences_array_.emplace_back(corr_ptr);

              local_result(7, 0) += config_.point2plane_constraint_gain * error * error;

              // The residual takes the partial derivative of the state
              Eigen::Matrix<double, 1, 6> dres_dx = Eigen::Matrix<double, 1, 6>::Zero();

              // The residual takes the partial derivative of rotation
              dres_dx.block<1, 3>(0, 0) = -plane_coeff.head(3).transpose() * curr_state_.pose.block<3, 3>(0, 0) * Sophus::SO3d::hat(p);

              // The residual takes the partial derivative of position
              dres_dx.block<1, 3>(0, 3) = plane_coeff.head(3).transpose();

              local_result.block(0, 0, 6, 6) += config_.point2plane_constraint_gain * dres_dx.transpose() * dres_dx;

              local_result.block(6, 0, 1, 6) += config_.point2plane_constraint_gain * dres_dx * error;
            }
          }
        }
        return local_result;
      },
      [](Eigen::Matrix<double, 8, 6> x, Eigen::Matrix<double, 8, 6> y) {return x + y;});

  //有效对齐个数effect_feat_num_
  effect_feat_num_ = correspondences_array_.size();

  H.block<6, 6>(IndexErrorOri, IndexErrorOri) += result_matrix.block<6, 6>(0, 0);
  b.block<6, 1>(IndexErrorOri, 0) += result_matrix.block<1, 6>(6, 0).transpose();

  return result_matrix(7, 0);
}

double LIO::ConstructImuPriorConstraints(Eigen::Matrix<double, 15, 15>& H,
                                         Eigen::Matrix<double, 15, 1>& b) 
{
  Sophus::SO3d ori_diff = Sophus::SO3d(prev_state_.pose.block<3, 3>(0, 0).transpose() * curr_state_.pose.block<3, 3>(0, 0));
  Eigen::Vector3d ori_error = ori_diff.log();

  Eigen::Matrix3d right_jacoiban_inv = Sophus::SO3d::jr_inv(ori_diff);

  Eigen::Matrix<double, 15, 15> jacobian = Eigen::Matrix<double, 15, 15>::Identity();
  jacobian.block<3, 3>(IndexErrorOri, IndexErrorOri) = right_jacoiban_inv;

  // LOG(INFO) << "imu jacobian: " << std::endl << jacobian;

  Eigen::Matrix<double, 15, 1> residual = Eigen::Matrix<double, 15, 1>::Zero();
  residual.block<3, 1>(IndexErrorOri, 0) = ori_error;
  residual.block<3, 1>(IndexErrorPos, 0) = curr_state_.pose.block<3, 1>(0, 3) - prev_state_.pose.block<3, 1>(0, 3);
  residual.block<3, 1>(IndexErrorVel, 0) = curr_state_.vel - prev_state_.vel;
  residual.block<3, 1>(IndexErrorBiasAcc, 0) = curr_state_.ba - prev_state_.ba;
  residual.block<3, 1>(IndexErrorBiasGyr, 0) = curr_state_.bg - prev_state_.bg;

  Eigen::Matrix<double, 15, 15> inv_P = P_.inverse();

  H += jacobian.transpose() * inv_P * jacobian;
  b += jacobian.transpose() * inv_P * residual;

  double errors = residual.transpose() * inv_P * residual;

  return errors;
}
//TODO::增加重力约束
//当前机器人的姿态角度估计，即旋转矩阵估计R
//根据旋转矩阵估计R，将机器人得到的加速度转化到世界坐标系下，在机器人没有上下运动的情况下，z方向的加速度应该是9.8左右
//存在的问题：机器人真的会上下运动啊，机器人实际的加速度为自身的运动加速度和重力加速度，这时候z方向的加速度就不是9.8了，也就是上述假设不成立啊
//能不能计算出来自身的运动加速度呢？感觉不太合理
double LIO::ConstructGravityConstraints(Eigen::Matrix<double, 15, 15>& H,
                                         Eigen::Matrix<double, 15, 1>& b) 
{
  //获取当前去掉ba的加速度值

  double errors = 0;
  return errors;
}

bool LIO::Predict(const double time,
                  const Eigen::Vector3d& acc_1,
                  const Eigen::Vector3d& gyr_1) {
  double dt = time - lio_time_;

  Eigen::Vector3d un_acc = Eigen::Vector3d::Zero();
  Eigen::Vector3d un_gyr = Eigen::Vector3d::Zero();
  //根据给定的加速度计和陀螺仪数据，使用NominalStateUpdate方法进行一次状态更新
  NominalStateUpdate(dt,         //积分时间
                     acc_0_,     //上一个IMU时刻的IMU加速度
                     acc_1,      //当前IMU时刻的IMU加速度
                     gyr_0_,     //上一个IMU时刻的IMU角速度
                     gyr_1,      //当前IMU时刻的角速度
                     prev_state_.pose,  //上一个IMU时刻的位姿
                     prev_state_.vel,   //上一个IMU时刻的速度
                     curr_state_.pose,  //IMU预测得到的当前IMU时刻的位姿
                     curr_state_.vel,   //IMU预测得到的当前IMU时刻的速度
                     un_acc,
                     un_gyr);
  //根据给定的加速度计和陀螺仪数据，使用ErrorStateUpdate方法进行一次状态更新
  ErrorStateUpdate(dt, acc_0_, acc_1, gyr_0_, gyr_1);//0表示上一个IMU时刻，1表示当前IMU时刻
  //如果配置项enable_undistort为真，则保存预测的位姿、误差数据以及速度信息，以便进行scan undistortion。将这些数据存储在pose_history_队列中
  if (config_.enable_undistort) {
    // 保存预测的位姿用于点云矫正
    PoseHistory ph;
    ph.time_ = time;
    ph.T_ = curr_state_.pose;
    ph.un_acc_ = un_acc;
    ph.un_gyr_ = un_gyr;
    ph.vel_ = curr_state_.vel;
    pose_history_.push_back(ph);
  }

  //更新状态，为下一次预测做准备。将当前状态设置为上次预测的状态，并将加速度计和陀螺仪数据分别更新为上次预测时的数据。同时，更新lio_time_
  prev_state_ = curr_state_;
  acc_0_ = acc_1;
  gyr_0_ = gyr_1;
  lio_time_ = time;

  return true;
}
//! 更新机器人的状态，特别是惯性系下的速度和位置
bool LIO::NominalStateUpdate(const double dt,
                             const Eigen::Vector3d& acc_0,
                             const Eigen::Vector3d& acc_1,
                             const Eigen::Vector3d& gyr_0,
                             const Eigen::Vector3d& gyr_1,
                             const Eigen::Matrix4d& T_prev,
                             const Eigen::Vector3d& vel_prev,
                             Eigen::Matrix4d& T_curr,
                             Eigen::Vector3d& vel_curr,
                             Eigen::Vector3d& un_acc,
                             Eigen::Vector3d& un_gyr) {
  // 根据陀螺仪数据计算机器人相对于当前姿态的旋转角速度，即单位向量un_gyr。然后，计算变换矩阵T_curr，使其满足T_curr = T_prev * SO3d::exp(un_gyr * dt)
  un_gyr = 0.5 * (gyr_0 + gyr_1) - curr_state_.bg;
  T_curr.block<3, 3>(0, 0) = T_prev.block<3, 3>(0, 0) * Sophus::SO3d::exp(un_gyr * dt).matrix();
  // 根据加速度数据计算机器人相对于当前姿态的加速力
  Eigen::Vector3d un_acc_0 = T_prev.block<3, 3>(0, 0) * (acc_0 - curr_state_.ba);
  Eigen::Vector3d un_acc_1 = T_curr.block<3, 3>(0, 0) * (acc_1 - curr_state_.ba);
  un_acc = 0.5 * (un_acc_0 + un_acc_1) - g_;

  // 根据上一个时刻的加速力和速度数据，计算当前时刻的速度数据
  vel_curr = vel_prev + un_acc * dt;
  // 根据上一个时刻的加速力、速度和位置数据，计算当前时刻的位置数据。同时，更新变换矩阵T_curr，使其满足T_curr = T_prev + vel_prev * dt + 0.5 * dt * dt * un_acc
  T_curr.block<3, 1>(0, 3) = T_prev.block<3, 1>(0, 3) + vel_prev * dt + 0.5 * dt * dt * un_acc;

  return true;
}

//! 更新机器人姿态估计状态。它是基于非线性ulates框架的，该框架使用广义力矩法（GMF）来估计机器人的姿态
bool LIO::ErrorStateUpdate(const double dt,
                           const Eigen::Vector3d& acc_0,
                           const Eigen::Vector3d& acc_1,
                           const Eigen::Vector3d& gyr_0,
                           const Eigen::Vector3d& gyr_1) {
  Eigen::Vector3d w = 0.5 * (gyr_0 + gyr_1) - curr_state_.bg;
  Eigen::Vector3d a0 = acc_0 - curr_state_.ba;
  Eigen::Vector3d a1 = acc_1 - curr_state_.ba;

  Eigen::Matrix3d w_x = Sophus::SO3d::hat(w).matrix();
  Eigen::Matrix3d a0_x = Sophus::SO3d::hat(a0).matrix();
  Eigen::Matrix3d a1_x = Sophus::SO3d::hat(a1).matrix();
  Eigen::Matrix3d I_w_x = Sophus::SO3d::exp(-w * dt).matrix();

  F_.setZero();
  // F_.block<3,3>(IndexErrorVel,IndexErrorOri) =
  //     -0.5 * dt * prev_state_.pose.block<3,3>(0,0) * a0_x
  //     -0.5 * dt * curr_state_.pose.block<3,3>(0,0) * a1_x *
  //     (Eigen::Matrix3d::Identity() - w_x * dt);
  F_.block<3, 3>(IndexErrorVel, IndexErrorOri) =
      -0.5 * dt * prev_state_.pose.block<3, 3>(0, 0) * a0_x -
      0.5 * dt * curr_state_.pose.block<3, 3>(0, 0) * a1_x *
          I_w_x;  // More accurate than above
  F_.block<3, 3>(IndexErrorVel, IndexErrorVel) = Eigen::Matrix3d::Identity();
  F_.block<3, 3>(IndexErrorVel, IndexErrorBiasAcc) =
      -0.5 *
      (prev_state_.pose.block<3, 3>(0, 0) +
       curr_state_.pose.block<3, 3>(0, 0)) *
      dt;
  F_.block<3, 3>(IndexErrorVel, IndexErrorBiasGyr) =
      0.5 * curr_state_.pose.block<3, 3>(0, 0) * a1_x * dt * dt;

  F_.block<3, 3>(IndexErrorPos, IndexErrorPos) = Eigen::Matrix3d::Identity();
  F_.block<3, 3>(IndexErrorPos, IndexErrorOri) =
      0.5 * dt * F_.block<3, 3>(IndexErrorVel, IndexErrorOri);
  F_.block<3, 3>(IndexErrorPos, IndexErrorVel) =
      Eigen::Matrix3d::Identity() * dt;
  F_.block<3, 3>(IndexErrorPos, IndexErrorBiasAcc) =
      0.5 * dt * F_.block<3, 3>(IndexErrorVel, IndexErrorBiasAcc);
  F_.block<3, 3>(IndexErrorPos, IndexErrorBiasGyr) =
      0.5 * dt * F_.block<3, 3>(IndexErrorVel, IndexErrorBiasGyr);

  // F_.block<3,3>(IndexErrorOri,IndexErrorOri) = Eigen::Matrix3d::Identity()
  // - w_x * dt;
  F_.block<3, 3>(IndexErrorOri, IndexErrorOri) =
      I_w_x;  // More accurate than above
  F_.block<3, 3>(IndexErrorOri, IndexErrorBiasGyr) =
      -Eigen::Matrix3d::Identity() * dt;

  F_.block<3, 3>(IndexErrorBiasAcc, IndexErrorBiasAcc) =
      Eigen::Matrix3d::Identity();
  F_.block<3, 3>(IndexErrorBiasGyr, IndexErrorBiasGyr) =
      Eigen::Matrix3d::Identity();

  B_.setZero();
  B_.block<3, 3>(IndexErrorVel, IndexNoiseAccLast) =
      0.5 * prev_state_.pose.block<3, 3>(0, 0) * dt;
  B_.block<3, 3>(IndexErrorVel, IndexNoiseGyrLast) =
      -0.25 * curr_state_.pose.block<3, 3>(0, 0) * a1_x * dt * dt;
  B_.block<3, 3>(IndexErrorVel, IndexNoiseAccCurr) =
      0.5 * curr_state_.pose.block<3, 3>(0, 0) * dt;
  B_.block<3, 3>(IndexErrorVel, IndexNoiseGyrCurr) =
      B_.block<3, 3>(IndexErrorVel, IndexNoiseGyrLast);

  B_.block<3, 3>(IndexErrorOri, IndexNoiseGyrLast) =
      0.5 * Eigen::Matrix3d::Identity() * dt;  // inaccuracy
  B_.block<3, 3>(IndexErrorOri, IndexNoiseGyrCurr) =
      B_.block<3, 3>(IndexErrorOri, IndexNoiseGyrLast);

  B_.block<3, 3>(IndexErrorPos, IndexNoiseAccLast) =
      0.5 * B_.block<3, 3>(IndexErrorVel, IndexNoiseAccLast) * dt;
  B_.block<3, 3>(IndexErrorPos, IndexNoiseGyrLast) =
      0.5 * B_.block<3, 3>(IndexErrorVel, IndexNoiseGyrLast) * dt;
  B_.block<3, 3>(IndexErrorPos, IndexNoiseAccCurr) =
      0.5 * B_.block<3, 3>(IndexErrorVel, IndexNoiseAccCurr) * dt;
  B_.block<3, 3>(IndexErrorPos, IndexNoiseGyrCurr) =
      B_.block<3, 3>(IndexErrorPos, IndexNoiseGyrLast);

  B_.block<3, 3>(IndexErrorBiasAcc, IndexNoiseBiasAcc) =
      Eigen::Matrix3d::Identity() * dt;
  B_.block<3, 3>(IndexErrorBiasGyr, IndexNoiseBiasGyr) =
      B_.block<3, 3>(IndexErrorBiasAcc, IndexNoiseBiasAcc);

  P_ = F_ * P_ * F_.transpose() + B_ * Q_ * B_.transpose();
  return true;
}

// Undistortion based on median integral
bool LIO::UndistortPointCloud(const double bag_time,
                              const double lidar_end_time,
                              CloudPtr& cloud_ptr) {
  Eigen::Matrix3d R_w_be = curr_state_.pose.block<3, 3>(0, 0);
  Eigen::Vector3d t_w_be = curr_state_.pose.block<3, 1>(0, 3);
  auto it_pt = cloud_ptr->points.end() - 1;
  bool finshed_flag = false;
  for (auto it_pose = pose_history_.end() - 1; it_pose != pose_history_.begin();
       --it_pose) {
    auto bi = it_pose - 1;
    auto bj = it_pose;
    Eigen::Matrix3d R_w_bi = bi->T_.block<3, 3>(0, 0);
    Eigen::Vector3d t_w_bi = bi->T_.block<3, 1>(0, 3);
    Eigen::Vector3d v_w_bi = bi->vel_;
    Eigen::Vector3d un_acc_bj = bj->un_acc_;
    Eigen::Vector3d un_gyr_bj = bj->un_gyr_;

    for (; (it_pt->curvature / (double)(1000) + bag_time) > bi->time_;--it_pt) 
    {
      double dt = (it_pt->curvature / (double)(1000) + bag_time) - bi->time_;

      Eigen::Matrix3d R_w_bk =
          R_w_bi * Sophus::SO3d::exp(un_gyr_bj * dt).matrix();
      Eigen::Vector3d t_w_bebk =
          t_w_bi + v_w_bi * dt + 0.5 * dt * dt * un_acc_bj - t_w_be;
      // point_K
      Eigen::Vector3d P_bk_bkK(it_pt->x, it_pt->y, it_pt->z);
      Eigen::Vector3d P_w_beK =
          R_w_be.transpose() * (R_w_bk * P_bk_bkK + t_w_bebk);

      it_pt->x = P_w_beK.x();
      it_pt->y = P_w_beK.y();
      it_pt->z = P_w_beK.z();

      if (it_pt == cloud_ptr->points.begin()) {
        finshed_flag = true;
        break;
      }
    }

    if (finshed_flag) {
      break;
    }
  }

  // Remove excess history imu_pose
  while (!pose_history_.empty() &&
         (pose_history_.front().time_ < lidar_end_time)) {
    pose_history_.pop_front();
  }

  return true;
}
/*
实现原理：
StaticInitialization函数通过读取IMU数据并计算平均值和协方差来初始化IMU的状态和协方差矩阵。
它首先计算加速度计和陀螺仪的平均值，然后通过矩阵分解（Schmidt正交化）计算初始姿态（roll、pitch和yaw）。接着计算初始速度、加速度计和陀螺仪的误差（bias和acc）。最后，它置初始状态、协方差矩阵和日志信息。

用途：
StaticInitialization函数通常在机器人或无人机启动时使用，以便在后续的动态跟踪或控制过程中使用初始化后的IMU数据。

注意事项：
确保IMU数据准确且未过滤。
确保IMU的安装位置和方向在初始化过程中保持不变。
如果使用加速度计和陀螺仪的噪声数据来初始化协方差矩阵，请确保噪声数据准确且未过滤。
如果使用多个初始化步骤（例如，先使用加速度计初始化姿态，然后使用陀螺仪初始化加速度计和陀螺仪的误差），请确保每个步骤的初始化误差尽可能小。
*/
bool LIO::StaticInitialization(SensorMeasurement& sensor_measurement) {
  if (first_imu_frame_) {
    const auto& acc = sensor_measurement.imu_buff_.front().linear_acceleration;
    const auto& gyr = sensor_measurement.imu_buff_.front().angular_velocity;
    imu_init_buff_.emplace_back(Eigen::Vector3d(acc.x, acc.y, acc.z),
                                Eigen::Vector3d(gyr.x, gyr.y, gyr.z));
  }

  for (const auto& imu_msg : sensor_measurement.imu_buff_) {
    Eigen::Vector3d acc(imu_msg.linear_acceleration.x,
                        imu_msg.linear_acceleration.y,
                        imu_msg.linear_acceleration.z);
    Eigen::Vector3d gyr(imu_msg.angular_velocity.x,
                        imu_msg.angular_velocity.y,
                        imu_msg.angular_velocity.z);

    imu_init_buff_.emplace_back(acc, gyr);
  }

  if (imu_init_buff_.size() < max_init_count_) {
    return false;
  }

  Eigen::Vector3d acc_cov, gyr_cov;
  ComputeMeanAndCovDiag(
      imu_init_buff_,
      mean_acc_,
      acc_cov,
      [](const std::pair<Eigen::Vector3d, Eigen::Vector3d>& imu_data) {
        return imu_data.first;
      });
  ComputeMeanAndCovDiag(
      imu_init_buff_,
      mean_gyr_,
      gyr_cov,
      [](const std::pair<Eigen::Vector3d, Eigen::Vector3d>& imu_data) {
        return imu_data.second;
      });

  /*
    通过施密特正交化方法计算初始姿态。 
    roll和pitch与重力方向对齐，但yaw是随机的。实现原理是通过计算三个轴的单位向量，然后通过施密特正交化公式计算初始旋转矩阵和四元数。

    计算z轴方向：首先计算重力向量，然后将其归一化。
    计算x轴方向：通过向量运算，找到与z轴正交的x轴方向。
    计算y轴方向：通过向量运算，将z轴与x轴正交的向量y轴方向。
    计算初始旋转矩阵R：将x轴、y轴和z轴的单位向量组合成3x3的矩阵。
    计算初始四元数q：将旋转矩阵R转换为四元数，并确保其归一化。
    更新当前状态中的位姿矩阵：将初始四元数转换为旋转矩阵，并将其转换为transpose()形式（因为这里使用的是列向量形式）。
    计算初始加速度计偏置：通过计算加速度计数据与实际加速度之间的差异，从而计算初始加速度计偏置。
    注意：这段代码使用了Eigen库进行矩阵和向量的操作，以及Sophus库中的SO3d类进行四元数操作。
  */
 
  Eigen::Vector3d z_axis = mean_acc_.normalized();
  Eigen::Vector3d e1(1, 0, 0);
  Eigen::Vector3d x_axis = e1 - z_axis * z_axis.transpose() * e1;
  x_axis.normalize();
  Eigen::Vector3d y_axis = Sophus::SO3d::hat(z_axis).matrix() * x_axis;
  y_axis.normalize();

  Eigen::Matrix3d init_R;
  init_R.block<3, 1>(0, 0) = x_axis;
  init_R.block<3, 1>(0, 1) = y_axis;
  init_R.block<3, 1>(0, 2) = z_axis;
  Eigen::Quaterniond init_q(init_R);
  curr_state_.pose.block<3, 3>(0, 0) =
      init_q.normalized().toRotationMatrix().transpose();

  Eigen::Vector3d init_ba = Eigen::Vector3d::Zero();
  ComputeMeanAndCovDiag(
      imu_init_buff_,
      init_ba,
      acc_cov,
      [this](const std::pair<Eigen::Vector3d, Eigen::Vector3d>& imu_data) {
        Eigen::Vector3d temp_ba =
            imu_data.first -
            curr_state_.pose.block<3, 3>(0, 0).transpose() * g_;
        return temp_ba;
      });

  //初始化位姿
  curr_state_.pose.block<3, 1>(0, 3).setZero();
  //初始化速度
  curr_state_.vel.setZero();
  //初始化bg
  curr_state_.bg = mean_gyr_;
  //初始化ba
  curr_state_.ba = init_ba;

  prev_state_ = curr_state_;
  //初始化P_
  P_.setIdentity();
  P_.block<3, 3>(IndexErrorOri, IndexErrorOri) =
      config_.init_ori_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorPos, IndexErrorPos) =
      config_.init_pos_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorVel, IndexErrorVel) =
      config_.init_vel_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorBiasAcc, IndexErrorBiasAcc) =
      config_.init_ba_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorBiasGyr, IndexErrorBiasGyr) =
      config_.init_bg_cov * Eigen::Matrix3d::Identity();
  //初始化Q_
  Q_.setIdentity();
  Q_.block<3, 3>(IndexNoiseAccLast, IndexNoiseAccLast) =
      config_.acc_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseGyrLast, IndexNoiseGyrLast) =
      config_.gyr_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseAccCurr, IndexNoiseAccCurr) =
      config_.acc_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseGyrCurr, IndexNoiseGyrCurr) =
      config_.gyr_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseBiasAcc, IndexNoiseBiasAcc) =
      config_.ba_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseBiasGyr, IndexNoiseBiasGyr) =
      config_.bg_cov * Eigen::Matrix3d::Identity();

  lio_time_ = sensor_measurement.imu_buff_.back().header.stamp.toSec();
  lio_init_ = true;

  LOG(INFO) << "imu static, mean_acc_: " << mean_acc_.transpose()
            << " init_ba: " << init_ba.transpose() << ", ori: " << std::endl
            << curr_state_.pose.block<3, 3>(0, 0);

  return true;
}

bool LIO::AHRSInitialization(SensorMeasurement& sensor_measurement) {
  const auto& back_imu = sensor_measurement.imu_buff_.back();

  if ((back_imu.orientation.w * back_imu.orientation.w +
       back_imu.orientation.x * back_imu.orientation.x +
       back_imu.orientation.y * back_imu.orientation.y +
       back_imu.orientation.z * back_imu.orientation.z) < 1.0) {
    LOG(ERROR) << "AHRS initalization falid, please use static initalizaiton!";
    return false;
  }

  Eigen::Quaterniond temp_q(back_imu.orientation.w,
                            back_imu.orientation.x,
                            back_imu.orientation.y,
                            back_imu.orientation.z);

  curr_state_.pose.block<3, 3>(0, 0) = temp_q.toRotationMatrix();

  curr_state_.pose.block<3, 1>(0, 3).setZero();

  curr_state_.vel.setZero();

  curr_state_.bg.setZero();

  curr_state_.ba.setZero();

  prev_state_ = curr_state_;

  P_.setIdentity();
  P_.block<3, 3>(IndexErrorOri, IndexErrorOri) =
      config_.init_ori_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorPos, IndexErrorPos) =
      config_.init_pos_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorVel, IndexErrorVel) =
      config_.init_vel_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorBiasAcc, IndexErrorBiasAcc) =
      config_.init_ba_cov * Eigen::Matrix3d::Identity();
  P_.block<3, 3>(IndexErrorBiasGyr, IndexErrorBiasGyr) =
      config_.init_bg_cov * Eigen::Matrix3d::Identity();

  Q_.setIdentity();
  Q_.block<3, 3>(IndexNoiseAccLast, IndexNoiseAccLast) =
      config_.acc_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseGyrLast, IndexNoiseGyrLast) =
      config_.gyr_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseAccCurr, IndexNoiseAccCurr) =
      config_.acc_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseGyrCurr, IndexNoiseGyrCurr) =
      config_.gyr_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseBiasAcc, IndexNoiseBiasAcc) =
      config_.ba_cov * Eigen::Matrix3d::Identity();
  Q_.block<3, 3>(IndexNoiseBiasGyr, IndexNoiseBiasGyr) =
      config_.bg_cov * Eigen::Matrix3d::Identity();

  lio_time_ = sensor_measurement.imu_buff_.back().header.stamp.toSec();
  lio_init_ = true;

  return true;
}

bool LIO::CorrectState(const State& state,
                       const Eigen::Matrix<double, 15, 1>& delta_x,
                       State& corrected_state) {
  // ori
  Eigen::Matrix3d delta_R =
      Sophus::SO3d::exp(delta_x.block<3, 1>(IndexErrorOri, 0)).matrix();
  // The normalization is employed after each update to pervent numerical
  // stability
  Eigen::Quaterniond temp_q(state.pose.block<3, 3>(0, 0) * delta_R);
  temp_q.normalize();
  corrected_state.pose.block<3, 3>(0, 0) = temp_q.toRotationMatrix();
  // pos
  corrected_state.pose.block<3, 1>(0, 3) =
      state.pose.block<3, 1>(0, 3) + delta_x.block<3, 1>(IndexErrorPos, 0);
  // vel
  corrected_state.vel = state.vel + delta_x.block<3, 1>(IndexErrorVel, 0);
  // ba
  corrected_state.ba = state.ba + delta_x.block<3, 1>(IndexErrorBiasAcc, 0);
  // bg
  corrected_state.bg = state.bg + delta_x.block<3, 1>(IndexErrorBiasGyr, 0);

  return true;
}

bool LIO::ComputeFinalCovariance(const Eigen::Matrix<double, 15, 1>& delta_x) {
  Eigen::Matrix<double, 15, 15> temp_P = final_hessian_.inverse();

  // project covariance
  Eigen::Matrix<double, 15, 15> L = Eigen::Matrix<double, 15, 15>::Identity();
  L.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() -
                        0.5 * Sophus::SO3d::hat(delta_x.block<3, 1>(0, 0));
  P_ = L * temp_P * L.transpose();

  return true;
}

bool LIO::IsConverged(const Eigen::Matrix<double, 15, 1>& delta_x) {
  return delta_x.lpNorm<Eigen::Infinity>() < transformation_epsilon_;
}


//构建图
void LIO::Graph_construction(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp) 
{
  //一共有多少组：group_num
  int group_num = correspondence.size()/per_gourp_num;
  //增加一组尾巴
  if(correspondence.size()%per_gourp_num!=0)
    group_num++;

  //遍历每一组
  for(size_t index = 0; index < group_num; index++)
  {
    //如果不是最后一组，则每一组的元素个数为per_gourp_num，否则取剩余的元素
    int size = per_gourp_num; 
    if(index == (group_num-1))
    {
      size = correspondence.size() - index*per_gourp_num;
    }

    //定义得分矩阵cmp_score
    Eigen::MatrixXd cmp_score; 
    cmp_score.resize(size, size);
    cmp_score.setZero();
      
    //生成得分矩阵cmp_score
    for (size_t i = 0; i < size; i++)
    {
      float score, src_dis, des_dis, dis;
      std::shared_ptr<Correspondence> c1, c2;

      if((i+(index)*per_gourp_num)>correspondence.size()) LOG(ERROR) << "(i+(index)*200)>correspondence.size()";
      
      c1 = correspondence[i+(index)*per_gourp_num];

      for (int j = i + 1; j < size; j++)
      {
        if((j+(index)*per_gourp_num)>correspondence.size()) LOG(ERROR) << "(j+(index)*200)>correspondence.size()";

        c2 = correspondence[j+(index)*per_gourp_num];

        //取出两组配对，分别计算源点云和目标点云中点云之间的距离，即src_dis和des_dis
        //A和A’对应，B和B’对应，则A与B之间的距离src_dis，A‘与B’之间的距离des_dis，应该是接近的，即dis很小
        src_dis = Distance(c1->mean_A, c2->mean_A);
        des_dis = Distance(c1->mean_B, c2->mean_B);

        //计算src_dis和des_dis之间的差，越小越好
        dis = abs(src_dis - des_dis);

        //! dis越小，得分score越接近1。这个得分表明了两组匹配之间的一致性
        score = exp(-dis * dis /alpha_dis_temp);

        score = (score < cmp_thresh) ? 0 : score;
        cmp_score(i, j) = score;
        cmp_score(j, i) = score;
      }
    }

    if (sc2)
    {
      Eigen::setNbThreads(6);
      cmp_score = cmp_score.cwiseProduct(cmp_score * cmp_score);
    }

    Index_Group temp;
    temp.current_index = index; //第几组
    temp.group =  cmp_score;    //该组的距离矩阵
    Graphs.push_back(temp);
/*
      //用于调试
      cv::Mat image(cmp_score.rows(), cmp_score.cols(), CV_32FC1, cmp_score.data());
      // 将Mat转换为8位图像
      cv::Mat image8u;
      image.convertTo(image8u, CV_8UC1);
      // 应用直方图均衡化以改善图像质量
      cv::Mat imageEq;
      cv::equalizeHist(image8u, imageEq);
      // 保存图像
      cv::imwrite("/home/ros/eigenMatrix_" + std::to_string(index) + "+.png", imageEq);
*/
  }
}

//构建图
void LIO::Graph_construction_V2(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp) 
{
    //int size = correspondence.size();
    
    int group_num = correspondence.size()/per_gourp_num;
    if(correspondence.size()%per_gourp_num!=0)
      group_num++;
    //std::cout<<"group_num:"<<group_num<<std::endl;
    // 多线程版本
    //tbb::parallel_for(tbb::blocked_range<size_t>(0, group_num),[&, this](tbb::blocked_range<size_t> r) 
    //{
      //for(size_t index = r.begin(); index < r.end(); ++index)
      for(size_t index = 0; index < group_num; index++)
      {
        //printf("index:%d\n",index);
        int size;
        if(index == (group_num-1))
        {
          size = correspondence.size() - index*per_gourp_num;
        }
        else
        {
          size = per_gourp_num;
        }

        Eigen::MatrixXd cmp_score; 
        //std::shared_ptr<Eigen::MatrixXd> cmp_score_shared;
        cmp_score.resize(size, size);
        cmp_score.setZero();

        for (size_t i = 0; i < size; i++)
        {
            float score, src_dis, des_dis, dis;
            std::shared_ptr<Correspondence> c1, c2;

            if((i+(index)*per_gourp_num)>correspondence.size())
              LOG(ERROR) << "(i+(index)*200)>correspondence.size()";

            c1 = correspondence[i+(index)*per_gourp_num];

            for (int j = i + 1; j < size; ++j)
            {
                if((j+(index)*per_gourp_num)>correspondence.size())
                  LOG(ERROR) << "(j+(index)*200)>correspondence.size()";

                c2 = correspondence[j+(index)*per_gourp_num];

                //取出两组配对，分别计算源点云和目标点云中点云之间的距离，即src_dis和des_dis
                //A和A’对应，B和B’对应，则A与B之间的距离src_dis，A‘与B’之间的距离des_dis，应该是接近的，即dis很小
                src_dis = Distance(c1->mean_A, c2->mean_A);
                des_dis = Distance(c1->mean_B, c2->mean_B);

                //计算src_dis和des_dis之间的差，越小越好
                dis = abs(src_dis - des_dis);

                //! dis越小，得分score越接近1。这个得分表明了两组匹配之间的一致性
                score = exp(-dis * dis /alpha_dis_temp);

                score = (score < cmp_thresh) ? 0 : 1;
                cmp_score(i, j) = score;
                cmp_score(j, i) = score;
            }
        }
        if (sc2)
        {
          Eigen::setNbThreads(6);
          cmp_score = cmp_score.cwiseProduct(cmp_score * cmp_score);
        }

        Index_Group temp;
        temp.current_index = index;
        temp.group =  cmp_score;
        Graphs.push_back(temp);
/*
          //用于调试
        cv::Mat image(cmp_score.rows(), cmp_score.cols(), CV_32FC1, cmp_score.data());
        // 将Mat转换为8位图像
        cv::Mat image8u;
        image.convertTo(image8u, CV_8UC1);
        // 应用直方图均衡化以改善图像质量
        cv::Mat imageEq;
        cv::equalizeHist(image8u, imageEq);
        // 保存图像
        cv::imwrite("/home/ros/eigenMatrix_" + std::to_string(index) + "+.png", imageEq);
*/
      }
}

//构建图
/*
void LIO::Graph_construction_old(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp) 
{
    //int size = correspondence.size();
   // int group_num = correspondence.size()/200+1;
    //std::cout<<"group_num:"<<group_num<<std::endl;
    // 多线程版本
    //tbb::parallel_for(tbb::blocked_range<size_t>(0, group_num),[&, this](tbb::blocked_range<size_t> r) 
    //{
      //for(size_t index = r.begin(); index < r.end(); ++index)
     // for(size_t index = 0; index < group_num; index++)
     // {
        //printf("index:%d\n",index);
        int size = correspondence.size();

        Eigen::MatrixXd cmp_score; 
        std::shared_ptr<Eigen::MatrixXd> cmp_score_shared;
        cmp_score.resize(size, size);
        cmp_score.setZero();
        
        //std::cout << "group_num: " << group_num <<", index: " << index << ", size: " << size << std::endl;
        // 多线程版本
        //tbb::parallel_for(
        //tbb::blocked_range<size_t>(0, size),
        //[&, this](tbb::blocked_range<size_t> r) 
        //{

        for (size_t i = 0; i < size; i++)
        {
            float score, src_dis, des_dis, dis;
            //iter_count++;
            std::shared_ptr<Correspondence> c1, c2;
            //printf("i:%d,index:%d,i+(index)*100:%d\n",i,index,i+(index)*100);
            c1 = correspondence[i];
            //std::cout << "i: " << i << std::endl;
            for (int j = i + 1; j < size; ++j)
            {
                //printf("j+(index)*100:%d\n",j+(index)*100);
                c2 = correspondence[j];
                //取出两组配对，分别计算源点云和目标点云中点云之间的距离，即src_dis和des_dis
                //A和A’对应，B和B’对应，则A与B之间的距离src_dis，A‘与B’之间的距离des_dis，应该是接近的，即dis很小
                src_dis = Distance(c1->mean_A, c2->mean_A);
                des_dis = Distance(c1->mean_B, c2->mean_B);
                //计算src_dis和des_dis之间的差，越小越好
                dis = abs(src_dis - des_dis);
                //alpha_dis = 10 * resolution;
                //! dis越小，得分score越接近1。这个得分表明了两组匹配之间的一致性
                score = exp(-dis * dis /alpha_dis_temp);
                //printf("src_dis: %f, des_dis: %f, dis: %f, score: %f\n", src_dis, des_dis,dis,score);
                //printf("score:{%f},cmp_thresh:{%f}\n",score,cmp_thresh);
                score = (score < cmp_thresh) ? 0 : 1;
                cmp_score(i, j) = score;
                cmp_score(j, i) = score;
            }
        }
        if (sc2)
        {
          Eigen::setNbThreads(6);
          cmp_score = cmp_score.cwiseProduct(cmp_score * cmp_score);
        }
        Graphs.push_back(cmp_score);
      //}
    //});
}
*/
void LIO::SearchGroups_old()
{
  //所有匹配对应的数量
  int all_total_num = correspondences_array_.size();
  std::chrono::time_point<std::chrono::system_clock> start, end;
  //! 构建图：correspondence为根据特征计算出来的对应关系,resolution为点云分辨率，sc2为true，cmp_thresh为0.99
  float cmp_thresh = 0.99;
  float resolution =0.05;
  start = std::chrono::system_clock::now();
  float alpha_dis = 0.5; //10*resolution
  float alpha_dis_temp = 2 * alpha_dis * alpha_dis;
  //第一個版本的构建图：完整圖
  Eigen::MatrixXd Graph = Graph_construction_old(correspondences_array_, resolution, sc2, cmp_thresh,alpha_dis_temp);

  if (Graph.norm() == 0 || Graph.rows() < 50) 
  {
      LOG(ERROR)<< "Graph.norm() == 0 || Graph.rows() < 50";
      return;
  }

  end = std::chrono::system_clock::now();
  elapsed_time = end - start;
  std::cout << " graph construction时间: " << elapsed_time.count() << std::endl;

  start = std::chrono::system_clock::now();
  //tbb::parallel_for(tbb::blocked_range<size_t>(0, Graphs.size()),[&, this](tbb::blocked_range<size_t> r) 
  //{
 // for (int index = r.begin(); index < r.end(); ++index)
    //{
  std::chrono::time_point<std::chrono::system_clock> group_start = std::chrono::system_clock::now();
      //const int current_index = Graphs[index].current_index;
      //const Eigen::MatrixXd Graph = Graphs[index].group;

      //std::cout << "Graph.rows(): " << Graph.rows() << ", Graph.cols(): " << Graph.cols() << std::endl;

  int total_num = correspondences_array_.size();
      //std::cout << "total_num: " << total_num << std::endl;

  //! 保存降采样点云每个点的链接关系（corre_index）和链接数量degree 
  tbb::concurrent_vector<int> degree(total_num, 0);
      //std::cout<<"total_num:"<<total_num<<std::endl;
      //std::cout<<"Graph.rows():"<<Graph.rows()<<",Graph.cols():"<<Graph.cols()<<std::endl;
      //std::cout<<"Graph:"<<Graph<<std::endl;
  for (int i = 0; i < total_num; ++i)
  {
    //Vote_exp t;
    //t.true_num = 0;
    //tbb::concurrent_vector<int> corre_index;
    for (int j = 0; j < total_num; j++)
    {
        if (i != j && Graph(i, j)>0.5) 
        {
            //! degree的值表示第i个匹配与多少个其他的匹配是一致的
            degree[i] ++;
            //! corre_index保存了i与哪些匹配是一致的
            //corre_index.push_back(j);
            //if (true_corre[j])
            //{
            //  t.true_num++;
            //}
        }
    }
  }
/*
      std::cout<<"degree:";
      for(int i=0;i<total_num;i++)
      {
        std::cout<<degree[i]<<" ";
      }
      std::cout<<std::endl;
*/
      //igraph
      igraph_t g;
      igraph_matrix_t g_mat;
      igraph_vector_t weights;
      igraph_vector_init(&weights, Graph.rows() * (Graph.cols() - 1) / 2);
      igraph_matrix_init(&g_mat, Graph.rows(), Graph.cols());

      //! 计算g_mat
      for (int i = 0; i < Graph.rows(); i++)
      {
          for (int j = i + 1; j < Graph.cols(); j++)
          {
              if (Graph(i, j))
              {
                  MATRIX(g_mat, i, j) = Graph(i, j);
                  MATRIX(g_mat, j, i) = MATRIX(g_mat, i, j);
              }
          }
      }

      igraph_set_attribute_table(&igraph_cattribute_table);
      igraph_vector_t weight;
      igraph_vector_init(&weight, 0);
      igraph_weighted_adjacency(&g, &g_mat, IGRAPH_ADJ_UNDIRECTED, &weight, IGRAPH_LOOPS_ONCE);

      igraph_vector_int_list_t cliques;
      igraph_vector_int_list_init(&cliques, 0);

      //! 从图g查找最大团cliques:利用修改的Bron-KerBosch算法，可以非常高效地搜索极大团
      //printf("搜索极大团：\n");
      //std::chrono::time_point<std::chrono::system_clock> group_searched_start = std::chrono::system_clock::now();
      //!10ms到20ms
      igraph_maximal_cliques(&g, &cliques, 3, 0); //3dlomatch 4 3dmatch; 3 Kitti  4
      //std::chrono::time_point<std::chrono::system_clock> group_searched_end = std::chrono::system_clock::now();
      //std::chrono::duration<double> group_searched_elapsed_time = group_searched_end - group_searched_start;
      //std::cout << " clique search: " << group_searched_elapsed_time.count() << std::endl;

      //end = std::chrono::system_clock::now();
      //elapsed_time = end - start;
      //int clique_num = igraph_vector_ptr_size(&cliques);
      int clique_num = igraph_vector_int_list_size(&cliques);
      if (clique_num == 0) {std::cout << " NO CLIQUES! " << std::endl;}
      printf("clique_num: %d\n", clique_num);

      //! 数据清理
      igraph_destroy(&g);
      igraph_matrix_destroy(&g_mat);

      std::vector<int>remain;
      //start = std::chrono::system_clock::now();
      for (int i = 0; i < clique_num; i++)
      {
          remain.push_back(i);
      }

      printf("remain.size(): %d\n", remain.size());
      node_cliques* N_C = new node_cliques[(int)total_num];
      std::chrono::time_point<std::chrono::system_clock> group_cut_start = std::chrono::system_clock::now();
      //! 時間不定，10ms到50ms之間
      find_largest_clique_of_node(Graph, &cliques, N_C, remain, total_num, max_est_num, descriptor);
      std::chrono::time_point<std::chrono::system_clock> group_cut_end = std::chrono::system_clock::now();
      std::chrono::duration<double> group_cut_elapsed_time = group_cut_end - group_cut_start;
      //LOG(INFO) <<current_index<<" 初始極大團數量："<<clique_num<< "剩餘數量：" <<remain.size()<<"耗時："<< group_cut_elapsed_time.count();

      //printf("remain.size(): %d\n", remain.size());
      //end = std::chrono::system_clock::now();
      //elapsed_time = end - start;
      //std::cout << " clique selection: " << elapsed_time.count() << std::endl;
      //! 合并简化后的极大团
      //start = std::chrono::system_clock::now();
      std::vector<int> save_corres_index_second;
      //遍历剩下的极大团（极大团索引为remain）
      int add_matches_num = 0;

      //std::chrono::time_point<std::chrono::system_clock> group_add_start = std::chrono::system_clock::now();
      //! 耗時忽略不計
      for (int i = 0; i < remain.size(); i++)
      {
          //遍历极大团中的节点
          igraph_vector_int_t* v = igraph_vector_int_list_get_ptr(&cliques, remain[i]);
          int group_size = igraph_vector_int_size(v);

          for (int j = 0; j < group_size; j++)
          {
            //节点索引为VECTOR(*v)[j]
            auto it = std::find(save_corres_index_second.begin(), save_corres_index_second.end(), VECTOR(*v)[j]);
            //如果这个匹配不在极大团里，并且其链接值大于50，则认为其是正确的，并加入Max_Group_second
            if (it == save_corres_index_second.end() && degree[VECTOR(*v)[j]]>50) 
            {
              //std::lock_guard<std::mutex> lock(Max_Group_mutex);
              std::shared_ptr<Correspondence> C = correspondences_array_[VECTOR(*v)[j]];
              Max_Group_second.push_back(C);
              save_corres_index_second.push_back(VECTOR(*v)[j]);
              add_matches_num++;
            }
          }
      }
      delete N_C;
      //std::chrono::time_point<std::chrono::system_clock> group_add_end = std::chrono::system_clock::now();
      //std::chrono::duration<double> group_add_elapsed_time = group_add_end - group_add_start;
      //std::cout << " clique add to Max_Group_second: " << group_add_elapsed_time.count() << std::endl;
      //释放内存空间
      igraph_vector_int_list_destroy(&cliques);
      std::chrono::time_point<std::chrono::system_clock> group_end = std::chrono::system_clock::now();
      std::chrono::duration<double> group_elapsed_time = group_end - group_start;
      std::cout << " clique selection: " << elapsed_time.count() <<"增加匹配數量："<<add_matches_num<< std::endl;
    //}
  //});

  end = std::chrono::system_clock::now();
  elapsed_time = end - start;
  LOG(INFO) << "raw: " << correspondences_array_.size() << " vs now: " << Max_Group_second.size() << "耗时：" << elapsed_time.count();

    /**/
  //用于调试
  //cv::Mat image(Graph.rows(), Graph.cols(), CV_32FC1, Graph.data());
  // 将Mat转换为8位图像
  //cv::Mat image8u;
  //image.convertTo(image8u, CV_8UC1);
  // 应用直方图均衡化以改善图像质量
  //cv::Mat imageEq;
  //cv::equalizeHist(image8u, imageEq);
  // 保存图像
  //cv::imwrite("/home/ros/eigenMatrix.png", imageEq);
  //cv::imwrite("/home/ros/eigenMatrix8u.png", image8u);

  //std::cout << " clique computation: " << elapsed_time.count() << std::endl
}

void LIO::SearchGroups()
{
  std::chrono::time_point<std::chrono::system_clock> start, end;
  start = std::chrono::system_clock::now();
  //! 构建图：correspondence为根据特征计算出来的对应关系,resolution为点云分辨率，sc2为true，cmp_thresh为0.99
  float cmp_thresh = 0.99;
  float resolution =0.05;
  float alpha_dis = 0.5; //10*resolution
  float alpha_dis_temp = 2 * alpha_dis * alpha_dis;
  //分组构建图（每組最多per_gourp_num个匹配），得到Graphs
  Graph_construction(correspondences_array_, resolution, sc2, cmp_thresh,alpha_dis_temp);
  end = std::chrono::system_clock::now();
  elapsed_time = end - start;
  std::cout << " graph construction时间: " << elapsed_time.count() << std::endl;

  start = std::chrono::system_clock::now();
  tbb::parallel_for(tbb::blocked_range<size_t>(0, Graphs.size()),[&, this](tbb::blocked_range<size_t> r) 
  {
    for (int index = r.begin(); index < r.end(); ++index)
    {
      std::chrono::time_point<std::chrono::system_clock> group_start = std::chrono::system_clock::now();
      //current_index表示第几组
      const int current_index = Graphs[index].current_index;
      //Graph为距离矩阵
      const Eigen::MatrixXd Graph = Graphs[index].group;

      if (Graph.norm() == 0 || Graph.rows() < 30) 
        continue;

      //! 这一组里的个数：不是最后一组，就是per_gourp_num个，如果是最后一组，就需要计算一下
      int total_num=per_gourp_num;
      if(current_index == (Graphs.size()-1))
        total_num = correspondences_array_.size() - current_index*per_gourp_num;

      //std::cout << "total_num: " << total_num << std::endl;

      if(Graph.rows()!=total_num) 
        LOG(ERROR)<<"Graph.rows()!=total_num";
        
      //! 保存降采样点云每个点的链接关系（corre_index）和链接数量degree 
      tbb::concurrent_vector<int> degree(total_num, 0);

      vector<Vote_exp> pts_degree;

      for (int i = 0; i < total_num; i++)
      {
        Vote_exp t;
        t.true_num = 0;
        vector<int> corre_index;

        for (int j = 0; j < total_num; j++)
        {
            if (i != j && Graph(i, j)) 
            {
                degree[i] ++;//! degree的值表示第i个匹配与多少个其他的匹配是一致的
                corre_index.push_back(j);
            }
        }

        t.index = i;
        t.degree = degree[i];
        t.corre_index = corre_index;
        pts_degree.push_back(t);
      }

      vector<Vote> cluster_factor;
      double sum_fenzi = 0;
      double sum_fenmu = 0;
      double max_score_value = 0;
      for (int i = 0; i < total_num; i++)
      {
        Vote t;
        double sum_i = 0;
        double wijk = 0;
        int index_size = pts_degree[i].corre_index.size();

        for (int j = 0; j < index_size; j++)
        {
            int a = pts_degree[i].corre_index[j];
            for (int k = j + 1; k < index_size; k++)
            {
                int b = pts_degree[i].corre_index[k];
                if (Graph(a, b)) 
                {
                    wijk += pow(Graph(i, a) * Graph(i, b) * Graph(a, b), 1.0 / 3); //wij + wik
                }
            }
        }

        if (degree[i] > 1)
        {
            double f1 = wijk;
            double f2 = degree[i] * (degree[i] - 1) * 0.5;
            sum_fenzi += f1;
            sum_fenmu += f2;
            double factor = f1 / f2;
            t.index = i;
            t.score = factor;
            cluster_factor.push_back(t);
            if(factor>max_score_value) max_score_value = factor;
        }
        else 
        {
            t.index = i;
            t.score = 0;
            cluster_factor.push_back(t);
        }
    }

    double average_factor = 0;
    for (size_t i = 0; i < cluster_factor.size(); i++)
    {
        average_factor += cluster_factor[i].score;
    }
    average_factor /= cluster_factor.size();

    double total_factor = sum_fenzi / sum_fenmu;

    vector<Vote_exp> pts_degree_bac;
    vector<Vote>cluster_factor_bac;
    pts_degree_bac.assign(pts_degree.begin(), pts_degree.end());
    cluster_factor_bac.assign(cluster_factor.begin(), cluster_factor.end());

    sort(cluster_factor.begin(), cluster_factor.end(), compare_vote_score);
    sort(pts_degree.begin(), pts_degree.end(), compare_vote_degree);

    Eigen::VectorXd cluster_coefficients;
    cluster_coefficients.resize(cluster_factor.size());
    for (size_t i = 0; i < cluster_factor.size(); i++)
    {
        cluster_coefficients[i] = cluster_factor[i].score;
    }

    int cnt = 0;
    double OTSU = 0;
    if (cluster_factor[0].score != 0)
    {
        OTSU = OTSU_thresh(cluster_coefficients);
    }
    double cluster_threshold = min(OTSU, min(average_factor, total_factor)); 
    //LOG(INFO)<<"cluster_threshold:"<<cluster_threshold;

    double weight_thresh = cluster_threshold;
    if (add_overlap)
      weight_thresh = 0.5;
    else
      weight_thresh = 0;

    //匹配置信度评分
    //if (!add_overlap)
    //{
    //    for (size_t i = 0; i < total_num; i++)
    //    {
    //        correspondences_array_[i]->score = cluster_factor_bac[i].score;
    //    }
    //}

    //igraph
    igraph_t g;
    igraph_matrix_t g_mat;
    igraph_vector_t weights;
    igraph_vector_init(&weights, Graph.rows() * (Graph.cols() - 1) / 2);
    igraph_matrix_init(&g_mat, Graph.rows(), Graph.cols());

    if (cluster_threshold > 3) // default 3 kitti-lc 2
    {
      float f = 10;
      while (1)
      {
        if (f * max(OTSU, total_factor) > cluster_factor[49].score)
            f -= 0.05;
        else
            break;
      }

      //! 计算g_mat
      for (int i = 0; i < Graph.rows(); i++)
      {
        if (cluster_factor_bac[i].score > f * max(OTSU, total_factor))
        {
          for (int j = i + 1; j < Graph.cols(); j++)
          {
            if (cluster_factor_bac[j].score > f * max(OTSU, total_factor))
            {
                MATRIX(g_mat, i, j) = Graph(i, j);
                MATRIX(g_mat, j, i) = MATRIX(g_mat, i, j);
            }
          }
        }
      }
    }
    else
    {
      for (int i = 0; i < Graph.rows(); i++)
      {
        for (int j = i + 1; j < Graph.cols(); j++)
        {
          MATRIX(g_mat, i, j) = Graph(i, j);
          MATRIX(g_mat, j, i) = MATRIX(g_mat, i, j);
        }
      } 
    }

    igraph_set_attribute_table(&igraph_cattribute_table);
    igraph_vector_t weight;
    igraph_vector_init(&weight, 0);
    igraph_weighted_adjacency(&g, &g_mat, IGRAPH_ADJ_UNDIRECTED, &weight, IGRAPH_LOOPS_ONCE);

    igraph_vector_int_list_t cliques;
    igraph_vector_int_list_init(&cliques, 0);

    //! 从图g查找最大团cliques:利用修改的Bron-KerBosch算法，可以非常高效地搜索极大团
    //std::chrono::time_point<std::chrono::system_clock> group_searched_start = std::chrono::system_clock::now();
    //!10ms到20ms
    igraph_maximal_cliques(&g, &cliques, 3, 0); //3dlomatch 4 3dmatch; 3 Kitti  4
    //std::chrono::time_point<std::chrono::system_clock> group_searched_end = std::chrono::system_clock::now();
    //std::chrono::duration<double> group_searched_elapsed_time = group_searched_end - group_searched_start;
    //std::cout << " clique search: " << group_searched_elapsed_time.count() << std::endl;

    //end = std::chrono::system_clock::now();
    //elapsed_time = end - start;
    //int clique_num = igraph_vector_ptr_size(&cliques);
    int clique_num = igraph_vector_int_list_size(&cliques);
    if (clique_num == 0) 
      LOG(ERROR) << " NO CLIQUES! ";
    //printf("clique_num: %d\n", clique_num);

    //! 数据清理
    igraph_destroy(&g);
    igraph_matrix_destroy(&g_mat);

    std::vector<int>remain;
    //start = std::chrono::system_clock::now();
    for (int i = 0; i < clique_num; i++)
    {
        remain.push_back(i);
    }

    //printf("remain.size(): %d\n", remain.size());
    node_cliques* N_C = new node_cliques[(int)total_num];
    //std::chrono::time_point<std::chrono::system_clock> group_cut_start = std::chrono::system_clock::now();
    //! 時間不定，10ms到50ms之間
    find_largest_clique_of_node(Graph, &cliques, N_C, remain, total_num, max_est_num, descriptor);
    //std::chrono::time_point<std::chrono::system_clock> group_cut_end = std::chrono::system_clock::now();
    //std::chrono::duration<double> group_cut_elapsed_time = group_cut_end - group_cut_start;
    //printf("remain.size(): %d\n", remain.size());
    //end = std::chrono::system_clock::now();
    //elapsed_time = end - start;
    //std::cout << " clique selection: " << elapsed_time.count() << std::endl;
    //! 合并简化后的极大团
    //start = std::chrono::system_clock::now();
    std::vector<int> save_corres_index_second;
    int add_matches_num = 0;

    //std::chrono::time_point<std::chrono::system_clock> group_add_start = std::chrono::system_clock::now();
    //! 耗時忽略不計
    for (int i = 0; i < remain.size(); i++)
    {
      //遍历极大团中的节点
      igraph_vector_int_t* v = igraph_vector_int_list_get_ptr(&cliques, remain[i]);
      int group_size = igraph_vector_int_size(v);

      for (int j = 0; j < group_size; j++)
      {
        //节点索引为VECTOR(*v)[j]
        //auto index_in_all  = VECTOR(*v)[j]+(current_index)*per_gourp_num;
        //LOG(INFO)<<"----VECTOR(*v)[j]----"<<VECTOR(*v)[j];
        auto it = std::find(save_corres_index_second.begin(), save_corres_index_second.end(), VECTOR(*v)[j]);
        //如果这个匹配不在极大团里，并且其链接值大于20，则认为其是正确的，并加入Max_Group_second
        if (it == save_corres_index_second.end() && degree[VECTOR(*v)[j]]>5) 
        {
          if((VECTOR(*v)[j]+(current_index)*per_gourp_num)>correspondences_array_.size()) LOG(ERROR)<<"VECTOR(*v)[j]:"<<VECTOR(*v)[j]<<"(current_index)*per_gourp_num:"<<(current_index)*per_gourp_num<<"correspondences_array_.size():"<<correspondences_array_.size();
          
          std::lock_guard<std::mutex> lock(Max_Group_mutex);
          std::shared_ptr<Correspondence> C = correspondences_array_[VECTOR(*v)[j]+(current_index)*per_gourp_num];
          C->score = cluster_factor[VECTOR(*v)[j]].score / max_score_value;
          //LOG(INFO)<<"max score: "<<max_score_value<<" VECTOR(*v)[j]: "<<VECTOR(*v)[j]<<" cluster_factor[VECTOR(*v)[j]]: "<<cluster_factor[VECTOR(*v)[j]].score;
          Max_Group_second.push_back(C);
          my_result_matrix += C->local_result;
          save_corres_index_second.push_back(VECTOR(*v)[j]);
          add_matches_num++;
        }
      }
    }
    delete N_C;
    //std::chrono::time_point<std::chrono::system_clock> group_add_end = std::chrono::system_clock::now();
    //std::chrono::duration<double> group_add_elapsed_time = group_add_end - group_add_start;
    //std::cout << " clique add to Max_Group_second: " << group_add_elapsed_time.count() << std::endl;
    //释放内存空间
    igraph_vector_int_list_destroy(&cliques);
    std::chrono::time_point<std::chrono::system_clock> group_end = std::chrono::system_clock::now();
    std::chrono::duration<double> group_elapsed_time = group_end - group_start;
    //std::cout << " clique selection: " << elapsed_time.count() <<"增加匹配數量："<<add_matches_num<< std::endl;
    }
  });

  end = std::chrono::system_clock::now();
  elapsed_time = end - start;
  LOG(INFO) << "raw: " << correspondences_array_.size() << " vs now: " << Max_Group_second.size() << "耗时：" << elapsed_time.count();

  //用于调试
  //cv::Mat image(Graph.rows(), Graph.cols(), CV_32FC1, Graph.data());
  // 将Mat转换为8位图像
  //cv::Mat image8u;
  //image.convertTo(image8u, CV_8UC1);
  // 应用直方图均衡化以改善图像质量
  //cv::Mat imageEq;
  //cv::equalizeHist(image8u, imageEq);
  // 保存图像
  //cv::imwrite("/home/ros/eigenMatrix.png", imageEq);
  //cv::imwrite("/home/ros/eigenMatrix8u.png", image8u);

  //std::cout << " clique computation: " << elapsed_time.count() << std::endl
}
/*
void LIO::SearchGroups_V2()
{
  //printf("Step1:创新点：构建对应关系correspondences_array_：【%d】\n", correspondences_array_.size());

  //所有匹配对应的数量
  int total_num = correspondences_array_.size();
  std::chrono::time_point<std::chrono::system_clock> start, end;
  //printf("Step2:搜索极大团：\n");
  //! 构建图：correspondence为根据特征计算出来的对应关系,resolution为点云分辨率，sc2为true，cmp_thresh为0.99
  float cmp_thresh = 0.8;
  float resolution =0.05;
  //start = std::chrono::system_clock::now();
  float alpha_dis = 0.5; //10*resolution
  float alpha_dis_temp = 2 * alpha_dis * alpha_dis;
  //printf("Step3:Graph_construction\n");
  Graph_construction_V2(correspondences_array_, resolution, sc2, cmp_thresh,alpha_dis_temp);
  //end = std::chrono::system_clock::now();
  //elapsed_time = end - start;
  //std::cout << " graph construction时间: " << elapsed_time.count() << std::endl;
  //std::cout<<"Graphs.size():"<<Graphs.size()<<std::endl;
  //for(int i=0;i<Graphs.size();i++)
  //{
  //  std::cout<<i<<" Graphs[].rows():"<<Graphs[i].rows()<<std::endl;
  //  std::cout<<i<<" Graphs[0].norm():"<<Graphs[i].norm()<<std::endl;
  //}

  printf("Graph_construction完成: %d\n",Graphs.size());

  start = std::chrono::system_clock::now();
  tbb::parallel_for(tbb::blocked_range<size_t>(0, Graphs.size()),[&, this](tbb::blocked_range<size_t> r) 
  {
    
    //遍历每个组，组号为i
    for (int index = r.begin(); index < r.end(); ++index)
    //for (int i = 0; i < Graphs.size(); ++i)
    {
      //printf("hello01\n");
      const int current_index = Graphs[index].current_index;
      const Eigen::MatrixXd graph_group = Graphs[index].group;
      //printf("hello02\n");
      if (graph_group.norm() == 0 || graph_group.rows()<20) 
      {
        LOG(WARNING) << current_index <<" Graph[i].norm() == 0"<<"graph_group rows: "<<graph_group.rows();
        continue;
      }
      //printf("hello03\n");
      //cv::Mat image(Graphs[i].rows(), Graphs[i].cols(), CV_32FC1, Graphs[i].data());
      // 将Mat转换为8位图像
      //cv::Mat image8u;
      //image.convertTo(image8u, CV_8UC1);
      // 应用直方图均衡化以改善图像质量
      //cv::Mat imageEq;
      //cv::equalizeHist(image8u, imageEq);
      // 保存图像
      //cv::imwrite("/home/ros/eigenMatrix.png", imageEq);
      //cv::imwrite("/home/ros/eigenMatrix8u.png", image8u);

      long clique_size = 0;
      std::vector<int> clique;
      Certificate certificate = NONE;
      //printf("hello04\n");
      //std::cout<< "graph_group.rows(): " << graph_group.rows() << ", graph_group.cols(): " << graph_group.cols() << std::endl;
      //std::cout<<graph_group<<std::endl;
      clipperplus::clipperplus_clique(graph_group, clique_size, clique, certificate);
      //printf("hello05\n");
      //std::cout << "第"<<i<<"个图 clique size: " << clique_size << std::endl;
      //std::cout << " clique: ";
      for (int j = 0; j < clique_size; j++)
      {
          //std::cout << clique[j] << "--";
          if((clique[j]+current_index*per_gourp_num)>correspondences_array_.size())
          {
              LOG(FATAL)<<"clique[j]+i*200 out of range";
              continue;
          }

          std::lock_guard<std::mutex> lock(Max_Group_mutex);
          std::shared_ptr<Correspondence> C = correspondences_array_[clique[j]+current_index*per_gourp_num];
          Max_Group_second.push_back(C);
      }
      //printf("hello06\n");
      //std::cout << std::endl;
      //std::cout << " certificate: " << certificate << std::endl;
    }
  });
    //printf("Step4:clipperplus_clique\n");
//用于调试
//! 合并简化后的极大团
end = std::chrono::system_clock::now();
elapsed_time = end - start;
std::cout << " 搜索、合并极大团: " << elapsed_time.count() <<"所有对应："<<correspondences_array_.size()<<"  極大團找到合适的匹配个数："<<Max_Group_second.size()<< std::endl;
//释放内存空间
}
*/
//构建图
Eigen::MatrixXd LIO::Graph_construction_old(tbb::concurrent_vector<std::shared_ptr<Correspondence>>& correspondence, float resolution, bool sc2, float cmp_thresh,float alpha_dis_temp) 
{
  
    int size = correspondence.size();
    Eigen::MatrixXd cmp_score; 
    cmp_score.resize(size, size);
    cmp_score.setZero();
    
    float score, src_dis, des_dis, dis;

    // 多线程版本
    tbb::parallel_for(
    tbb::blocked_range<size_t>(0, correspondence.size()),
    [&, this](tbb::blocked_range<size_t> r) 
    {
      for (size_t i = r.begin(); i < r.end(); ++i)
      {
          //iter_count++;
          std::shared_ptr<Correspondence> c1, c2;
          c1 = correspondence[i];
          //std::cout << "i: " << i << std::endl;
          for (int j = i + 1; j < correspondence.size(); ++j)
          {
              c2 = correspondence[j];
              //取出两组配对，分别计算源点云和目标点云中点云之间的距离，即src_dis和des_dis
              //A和A’对应，B和B’对应，则A与B之间的距离src_dis，A‘与B’之间的距离des_dis，应该是接近的，即dis很小
              src_dis = Distance(c1->mean_A, c2->mean_A);
              des_dis = Distance(c1->mean_B, c2->mean_B);
              //计算src_dis和des_dis之间的差，越小越好
              dis = abs(src_dis - des_dis);
              //alpha_dis = 10 * resolution;
              //! dis越小，得分score越接近1。这个得分表明了两组匹配之间的一致性
              score = exp(-dis * dis /alpha_dis_temp);
              //printf("src_dis: %f, des_dis: %f, dis: %f, score: %f\n", src_dis, des_dis,dis,score);
              //printf("score:{%f},cmp_thresh:{%f}\n",score,cmp_thresh);
              score = (score < cmp_thresh) ? 0 : score;
              cmp_score(i, j) = score;
              cmp_score(j, i) = score;
          }
      }
      if (sc2)
      {
        Eigen::setNbThreads(6);
        cmp_score = cmp_score.cwiseProduct(cmp_score * cmp_score);
      }
    });
    return cmp_score;
}
